├── .catkin_workspace ├── README.md ├── picture ├── acc_allan.jpg ├── gyro_allan.jpg ├── imu_integretion_in_euler_and_mid1.png └── imu_integretion_in_euler_and_mid2.png └── src ├── CMakeLists.txt ├── code_utils ├── CMakeLists.txt ├── README.md ├── include │ └── code_utils │ │ ├── backward.hpp │ │ ├── cv_utils.h │ │ ├── cv_utils │ │ ├── cv_type.hpp │ │ ├── dlt │ │ │ └── dlt.h │ │ ├── pnp │ │ │ ├── linearpnp.h │ │ │ ├── nonlinearpnp.h │ │ │ ├── pnp.h │ │ │ └── utils.h │ │ ├── randomcolor.h │ │ └── scalartodata.h │ │ ├── eigen_utils.h │ │ ├── math_utils │ │ ├── Polynomial.h │ │ ├── acos_fast.h │ │ └── math_utils.h │ │ ├── ros_utils.h │ │ └── sys_utils │ │ ├── cvmat_file_io.hpp │ │ ├── eigen_file_io.hpp │ │ ├── float_equal.hpp │ │ ├── printcolor.hpp │ │ ├── tic_toc.h │ │ └── timeinseconds.hpp ├── package.xml └── src │ ├── cv_utils.cc │ ├── cv_utils │ ├── dlt │ │ └── dlt.cpp │ └── pnp │ │ ├── linearpnp.cpp │ │ ├── nonlinearpnp.cpp │ │ └── pnp.cpp │ ├── mat_io_test.cpp │ ├── math_utils │ └── Polynomial.cpp │ ├── poly_test.cpp │ └── sumpixel_test.cpp ├── imu_utils ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── _config.yml ├── data │ ├── A3_imu_param.yaml │ ├── data_A3_acc_t.txt │ ├── data_A3_acc_x.txt │ ├── data_A3_acc_y.txt │ ├── data_A3_acc_z.txt │ ├── data_A3_gyr_t.txt │ ├── data_A3_gyr_x.txt │ ├── data_A3_gyr_y.txt │ ├── data_A3_gyr_z.txt │ ├── data_A3_sim_acc_t.txt │ ├── data_A3_sim_acc_x.txt │ ├── data_A3_sim_acc_y.txt │ ├── data_A3_sim_acc_z.txt │ ├── data_A3_sim_gyr_t.txt │ ├── data_A3_sim_gyr_x.txt │ ├── data_A3_sim_gyr_y.txt │ └── data_A3_sim_gyr_z.txt ├── figure │ ├── acc.jpg │ └── gyr.jpg ├── launch │ ├── 16448.launch │ ├── A3.launch │ ├── gx4.launch │ ├── n3.launch │ ├── sim_imu.launch │ ├── tum.launch │ └── xsens.launch ├── package.xml ├── scripts │ ├── all_of_draw_allan.m │ ├── all_of_draw_allan_acc.m │ ├── allan_with_degree.m │ ├── avg_of_all_sensors.m │ ├── avg_of_all_sensors_acc.m │ ├── draw_allan.m │ └── ideal_allan.m └── src │ ├── acc_lib │ ├── allan_acc.cpp │ ├── allan_acc.h │ ├── fitallan_acc.cpp │ └── fitallan_acc.h │ ├── gyr_lib │ ├── allan_gyr.cpp │ ├── allan_gyr.h │ ├── fitallan_gyr.cpp │ └── fitallan_gyr.h │ ├── imu_an.cpp │ ├── type.h │ └── utils.h └── vio_data_simulation ├── CMakeLists.txt ├── README.md ├── package.xml ├── rviz └── vio_simulation.rviz └── src ├── gener_alldata.cpp ├── imu.cpp ├── imu.h ├── param.cpp ├── param.h ├── utilities.cpp └── utilities.h /.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # imu_data_simulation 2 | This is a ros package that can generate imu data based on the imu error model and test the imu euler and mid integretion algorithm based on the imu motion model, and visualize the trajectory in rviz. 3 | 4 | 1. 计算IMU方差 5 | 通过IMU误差模型，利用程序增加加速度计和陀螺仪的高斯白噪声和bias的噪声，生成静止状态下的IMU数据，然后通过程序计算方差。加速度计和陀螺仪Allan方差曲线如下图所示： 6 | ![acc_allan](https://github.com/robosu12/imu_data_simulation/blob/master/picture/acc_allan.jpg) 7 | ![gyro_allan](https://github.com/robosu12/imu_data_simulation/blob/master/picture/gyro_allan.jpg) 8 | 程序中计算得到的是离散时间的噪声，连续时间到离散时间需要除以根号下采样频率。 9 | 10 | 总结：从标定结果可以看到，经过转换，加速度计和陀螺仪的高斯白噪声测量值和设定值基本接近，量级在百分之一左右；而加速度计和陀螺仪的bias噪声标定结果与设定结果相差较大，这是由于噪声的量级太小，在万分之一， 所以标定结果会有较大误差；所以这也说明在实际VIO应用中，需要将 IMU的bias加入到后端优化中进行实时估计。 11 | 12 | 2. 进行IMU积分计算运动轨迹 13 | 程序为一个ROS的工作空间，本人在贺一加老师提供的代码基础上，将生成IMU数据的代码封装成一个类，可以实时生成运动的imu数据，并且将生成的IMU数据实时进行积分，并且添加了IMU，PATH，ODOMETRY等topic的发布代码，可以在rviz中实时可视化IMU姿态，真实轨迹，积分轨迹，并且提供了已经配置好的rviz文件。 14 | 15 | 程序运行流程： 16 | 17 | a. cd vio-data-simulation-ws 18 | 19 | b. catkin_make 20 | 21 | c. source devel/setup.bash 22 | 23 | d. roscore 24 | 25 | e. rviz -d rviz/vio_simulation.rviz 26 | 27 | f. rosrun vio_data_simulation vio_data_simulation_node 28 | 29 | 30 | 两次运行结果如下图所示，其中绿色线为真实轨迹，红色线为欧拉积分轨迹，白色线为中值积分轨迹： 31 | ![imu_integration_1](https://github.com/robosu12/imu_data_simulation/blob/master/picture/imu_integretion_in_euler_and_mid1.png) 32 | ![imu_integration_2](https://github.com/robosu12/imu_data_simulation/blob/master/picture/imu_integretion_in_euler_and_mid2.png) 33 | 34 | 总结：可以看出，刚开始积分得到的轨迹和实际轨迹很接近，但是很快就飘走了。实际这是很正常的，而且这样的结果已经比较理想，因为仿真产生的IMU数据只考虑了噪声，并没有考虑到地球自转，加速度计与陀螺仪的耦合影响等其它因素的影响；在实际应用中，对于低端IMU进行纯积分，得到的轨迹几秒钟之后立刻就发散了，所以将视觉和IMU，甚至更多传感器融合到一起，取长补短，得到一个鲁棒的SLAM算法是目前研究的热点。 35 | 36 | -------------------------------------------------------------------------------- /picture/acc_allan.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosu12/imu_data_simulation/6fdcd99a58dda3f0b5c68e60b575d0b407e9d0e8/picture/acc_allan.jpg -------------------------------------------------------------------------------- /picture/gyro_allan.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosu12/imu_data_simulation/6fdcd99a58dda3f0b5c68e60b575d0b407e9d0e8/picture/gyro_allan.jpg -------------------------------------------------------------------------------- /picture/imu_integretion_in_euler_and_mid1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosu12/imu_data_simulation/6fdcd99a58dda3f0b5c68e60b575d0b407e9d0e8/picture/imu_integretion_in_euler_and_mid1.png -------------------------------------------------------------------------------- /picture/imu_integretion_in_euler_and_mid2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosu12/imu_data_simulation/6fdcd99a58dda3f0b5c68e60b575d0b407e9d0e8/picture/imu_integretion_in_euler_and_mid2.png -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/code_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(code_utils) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -fPIC -fopenmp") 7 | 8 | find_package(catkin REQUIRED 9 | roscpp 10 | std_msgs 11 | ) 12 | 13 | #set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") 14 | find_package(Eigen3 REQUIRED) 15 | find_package(OpenCV REQUIRED) 16 | find_package(Ceres REQUIRED) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS include 20 | LIBRARIES polynomial cv_utils pnp 21 | CATKIN_DEPENDS roscpp std_msgs 22 | # DEPENDS system_lib 23 | ) 24 | 25 | include_directories( 26 | ${catkin_INCLUDE_DIRS} 27 | ${EIGEN3_INCLUDE_DIR} 28 | ) 29 | include_directories("include") 30 | 31 | add_library(polynomial STATIC 32 | src/math_utils/Polynomial.cpp) 33 | target_link_libraries(polynomial ${Boost_LIBRARIES} ) 34 | 35 | add_library(pnp 36 | src/cv_utils/dlt/dlt.cpp 37 | src/cv_utils/pnp/pnp.cpp 38 | src/cv_utils/pnp/linearpnp.cpp 39 | src/cv_utils/pnp/nonlinearpnp.cpp) 40 | target_link_libraries(pnp ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 41 | 42 | add_library(cv_utils STATIC 43 | src/cv_utils.cc 44 | ) 45 | target_link_libraries(cv_utils ${Boost_LIBRARIES} ${OpenCV_LIBS} ) 46 | 47 | add_executable(matIO_test src/mat_io_test.cpp ) 48 | target_link_libraries(matIO_test dw ${OpenCV_LIBS}) 49 | 50 | add_executable(sumpixel_test src/sumpixel_test.cpp ) 51 | target_link_libraries(sumpixel_test dw ${OpenCV_LIBS}) 52 | -------------------------------------------------------------------------------- /src/code_utils/README.md: -------------------------------------------------------------------------------- 1 | # code_utils 2 | my code utils 3 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/cv_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef CV_UTLIS_H 2 | #define CV_UTLIS_H 3 | 4 | #include 5 | 6 | namespace cv_utils 7 | { 8 | namespace fisheye 9 | { 10 | class PreProcess 11 | { 12 | public: 13 | PreProcess( ); 14 | PreProcess( const cv::Size _raw_image_size, 15 | const cv::Size _roi_size, 16 | const cv::Point _center, 17 | const float _resize_scale ); 18 | void resetPreProcess( cv::Size _roi_size, cv::Point _center, float _resize_scale ); 19 | 20 | cv::Mat do_preprocess( cv::Mat image_input ); 21 | cv::Point2f preprocessPoint( const cv::Point2f& pt_in ); 22 | 23 | float resize_scale; 24 | int roi_row_start; 25 | int roi_col_start; 26 | int roi_row_end; 27 | int roi_col_end; 28 | 29 | bool is_preprocess; 30 | }; 31 | } 32 | } 33 | #endif // CV_UTLIS_H 34 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/cv_utils/cv_type.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CV_TYPE_HPP 2 | #define CV_TYPE_HPP 3 | 4 | #include 5 | 6 | namespace cv 7 | { 8 | 9 | #define CV_8UC12 CV_MAKETYPE( CV_8U, 12 ) 10 | 11 | #define CV_8SC12 CV_MAKETYPE( CV_8S, 12 ) 12 | #define CV_8SC16 CV_MAKETYPE( CV_8S, 16 ) 13 | #define CV_8SC24 CV_MAKETYPE( CV_8S, 24 ) 14 | #define CV_8SC32 CV_MAKETYPE( CV_8S, 32 ) 15 | 16 | #define CV_16SC12 CV_MAKETYPE( CV_8S, 12 ) 17 | 18 | #define CV_32SC12 CV_MAKETYPE( CV_32S, 12 ) 19 | #define CV_32SC24 CV_MAKETYPE( CV_32S, 24 ) 20 | 21 | #define CV_32FC5 CV_MAKETYPE( CV_32F, 5 ) 22 | 23 | typedef Vec< uchar, 12 > Vec12b; 24 | 25 | typedef Vec< short, 12 > Vec12s; 26 | typedef Vec< short, 16 > Vec16s; 27 | typedef Vec< short, 24 > Vec24s; 28 | typedef Vec< short, 32 > Vec32s; 29 | 30 | typedef Vec< char, 3 > Vec3c; 31 | typedef Vec< char, 4 > Vec4c; 32 | 33 | typedef Vec< char, 12 > Vec12c; 34 | typedef Vec< char, 16 > Vec16c; 35 | typedef Vec< char, 24 > Vec24c; 36 | typedef Vec< char, 32 > Vec32c; 37 | 38 | typedef Vec< int, 12 > Vec12i; 39 | typedef Vec< int, 16 > Vec16i; 40 | typedef Vec< int, 24 > Vec24i; 41 | typedef Vec< int, 32 > Vec32i; 42 | 43 | typedef Vec< float, 5 > Vec5f; 44 | } 45 | 46 | #endif // CV_TYPE_HPP 47 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/cv_utils/dlt/dlt.h: -------------------------------------------------------------------------------- 1 | #ifndef DLT_H 2 | #define DLT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace cv 10 | { 11 | 12 | class DLT 13 | { 14 | public: 15 | DLT( const std::vector< Eigen::Vector3d >& pts_2, const std::vector< Eigen::Vector3d >& pts_3 ); 16 | 17 | public: 18 | Eigen::Matrix3d getR( ) const { return R; } 19 | Eigen::Vector3d getT( ) const { return T; } 20 | bool solved( ) const { return m_solve_ok; } 21 | 22 | private: 23 | void readPointsPlanar( const std::vector< Eigen::Vector3d >& pts_2, 24 | const std::vector< Eigen::Vector3d >& pts_3 ); 25 | void solveDLT( ); 26 | 27 | private: 28 | Eigen::Matrix3d R; 29 | Eigen::Matrix3d RR; 30 | Eigen::Vector3d T; 31 | 32 | Eigen::MatrixXd M; 33 | Eigen::MatrixXd mat_tmp; 34 | int m_num_points; 35 | bool m_solve_ok; 36 | 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | }; 40 | } 41 | #endif // DLT_H 42 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/cv_utils/pnp/linearpnp.h: -------------------------------------------------------------------------------- 1 | #ifndef Homography_H 2 | #define Homography_H 3 | 4 | #include "utils.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace cv 12 | { 13 | class Homography 14 | { 15 | public: 16 | Homography( const std::vector< Eigen::Vector3d >& pts_2, const std::vector< Eigen::Vector3d >& pts_3 ); 17 | // ~Homography( ) {} 18 | 19 | public: 20 | Eigen::Matrix3d getR( ) const { return R; } 21 | Eigen::Vector3d getT( ) const { return T; } 22 | 23 | private: 24 | void readPointsPlanar( const std::vector< Eigen::Vector3d >& pts_2, 25 | const std::vector< Eigen::Vector3d >& pts_3 ); 26 | void solvePnP( ); 27 | 28 | private: 29 | Eigen::Matrix3d R; 30 | Eigen::Vector3d T; 31 | 32 | Eigen::MatrixXd M; 33 | Eigen::MatrixXd mat_tmp; 34 | 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | }; 38 | } 39 | 40 | #endif // Homography_H 41 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/cv_utils/pnp/nonlinearpnp.h: -------------------------------------------------------------------------------- 1 | #ifndef NONLINEARPNP_H 2 | #define NONLINEARPNP_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace cv 12 | { 13 | 14 | class Utility 15 | { 16 | public: 17 | template< typename Derived > 18 | static Eigen::Matrix< typename Derived::Scalar, 3, 3 > 19 | skewSymmetric( const Eigen::MatrixBase< Derived >& q ) 20 | { 21 | Eigen::Matrix< typename Derived::Scalar, 3, 3 > ans; 22 | ans << typename Derived::Scalar( 0 ), -q( 2 ), q( 1 ), q( 2 ), 23 | typename Derived::Scalar( 0 ), -q( 0 ), -q( 1 ), q( 0 ), typename Derived::Scalar( 0 ); 24 | return ans; 25 | } 26 | }; 27 | 28 | class NonlinearPnP 29 | { 30 | class ReprojectionError 31 | { 32 | public: 33 | ReprojectionError( const Eigen::Vector3d& image_point, const Eigen::Vector3d& scene_point ); 34 | 35 | template< typename T > 36 | bool operator( )( const T* const ex_paramt, T* residuals ) const 37 | { 38 | 39 | Eigen::Matrix< T, 3, 1 > _t_c; 40 | _t_c[0] = ex_paramt[0]; 41 | _t_c[1] = ex_paramt[1]; 42 | _t_c[2] = ex_paramt[2]; 43 | 44 | Eigen::Quaternion< T > _q_cw( ex_paramt[6], ex_paramt[3], ex_paramt[4], ex_paramt[5] ); 45 | _q_cw.normalize( ); 46 | 47 | Eigen::Matrix< T, 3, 1 > p_w; 48 | p_w[0] = T( scene_point_.x( ) ); 49 | p_w[1] = T( scene_point_.y( ) ); 50 | p_w[2] = T( scene_point_.z( ) ); 51 | 52 | Eigen::Matrix< T, 3, 1 > p_c; 53 | p_c = _q_cw * p_w + _t_c; 54 | p_c.normalize( ); 55 | 56 | T r_x = p_c( 0 ) - T( image_point_( 0 ) ); 57 | T r_y = p_c( 1 ) - T( image_point_( 1 ) ); 58 | T r_z = p_c( 2 ) - T( image_point_( 2 ) ); 59 | 60 | // clang-format off 61 | residuals[0] = T( tangent_base( 0, 0 ) ) * r_x 62 | + T( tangent_base( 0, 1 ) ) * r_y 63 | + T( tangent_base( 0, 2 ) ) * r_z; 64 | residuals[1] = T( tangent_base( 1, 0 ) ) * r_x 65 | + T( tangent_base( 1, 1 ) ) * r_y 66 | + T( tangent_base( 1, 2 ) ) * r_z; 67 | // clang-format on 68 | 69 | return true; 70 | } 71 | 72 | Eigen::Vector3d image_point_; // 2D to 3D point on image 73 | Eigen::Vector3d scene_point_; // 3D corresponding point 74 | Eigen::Matrix< double, 2, 3 > tangent_base; 75 | 76 | public: 77 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 78 | }; 79 | 80 | public: 81 | NonlinearPnP( const Eigen::Matrix3d& _R_initial, 82 | const Eigen::Vector3d& _T_initial, 83 | const std::vector< Eigen::Vector3d >& image_point, 84 | const std::vector< Eigen::Vector3d >& scene_point ); 85 | ~NonlinearPnP( ) {} 86 | 87 | bool getRT( Eigen::Vector3d& T_out, Eigen::Quaterniond& q_out ); 88 | 89 | bool solved; 90 | Eigen::Quaterniond q; 91 | Eigen::Vector3d T; 92 | double data[9]; 93 | int point_num; 94 | }; 95 | 96 | class ProjectionFactor : public ceres::SizedCostFunction< 2, 7 > 97 | { 98 | public: 99 | ProjectionFactor( const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j ); 100 | virtual bool Evaluate( const double* const* ex_paramt, double* residuals, double** jacobians ) const; 101 | void check( double** parameters ) {} 102 | 103 | Eigen::Vector3d image_point_, scene_point_; 104 | Eigen::Matrix< double, 2, 3 > tangent_base; 105 | static Eigen::Matrix2d sqrt_info; 106 | static double sum_t; 107 | }; 108 | } 109 | #endif // NONLINEARPNP_H 110 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/cv_utils/pnp/pnp.h: -------------------------------------------------------------------------------- 1 | #ifndef INITPNP_H 2 | #define INITPNP_H 3 | 4 | #include "../dlt/dlt.h" 5 | #include "linearpnp.h" 6 | #include "nonlinearpnp.h" 7 | 8 | namespace cv 9 | { 10 | class Pnp 11 | { 12 | public: 13 | Pnp( const std::vector< Eigen::Vector3d >& image_point, // 14 | const std::vector< Eigen::Vector3d >& scene_point ); 15 | 16 | Pnp( const std::vector< Eigen::Vector3d >& image_point, 17 | const std::vector< Eigen::Vector3d >& scene_point, 18 | Eigen::Quaterniond& q_dst, 19 | Eigen::Vector3d& T_dst ); 20 | 21 | Pnp( const std::vector< Eigen::Vector3d >& image_point, 22 | const std::vector< Eigen::Vector3d >& scene_point, 23 | const Eigen::Quaterniond& q_init, 24 | const Eigen::Vector3d& T_init, 25 | Eigen::Quaterniond& q_dst, 26 | Eigen::Vector3d& T_dst ); 27 | 28 | bool getRT( Eigen::Quaterniond& q_dst, Eigen::Vector3d& T_dst ); 29 | 30 | private: 31 | bool solved; 32 | NonlinearPnP* npnp; 33 | }; 34 | } 35 | #endif // INITPNP_H 36 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/cv_utils/pnp/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILS_H 2 | #define UTILS_H 3 | 4 | namespace math_utils 5 | { 6 | 7 | template< typename T > 8 | inline T 9 | max_in_three( T a, T b, T c ) 10 | { 11 | return a >= b ? ( a >= c ? a : c ) : ( b >= c ? b : c ); 12 | } 13 | 14 | template< typename T > 15 | inline T 16 | max_index_in_three( T a, T b, T c ) 17 | { 18 | return a >= b ? ( a >= c ? T( 0 ) : T( 2 ) ) : ( b >= c ? T( 1 ) : T( 2 ) ); 19 | } 20 | } 21 | #endif // UTILS_H 22 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/cv_utils/randomcolor.h: -------------------------------------------------------------------------------- 1 | #ifndef RANDOMCOLOR_H 2 | #define RANDOMCOLOR_H 3 | 4 | #include 5 | 6 | namespace cv 7 | { 8 | 9 | class RandomColor3 10 | { 11 | public: 12 | RandomColor3( ) { randColor( ); } 13 | 14 | void randColor( ) 15 | { 16 | color0 = rand( ) % 256; 17 | color1 = rand( ) % 256; 18 | color2 = rand( ) % 256; 19 | } 20 | cv::Scalar getColor( ) { return cv::Scalar( color0, color1, color2 ); } 21 | cv::Scalar getrandColor( ) 22 | { 23 | randColor( ); 24 | return cv::Scalar( color0, color1, color2 ); 25 | } 26 | cv::Vec3b getColorVec( ) 27 | { 28 | return cv::Vec3b( ( uchar )color0, ( uchar )color1, ( uchar )color2 ); 29 | } 30 | 31 | private: 32 | int color0; 33 | int color1; 34 | int color2; 35 | }; 36 | 37 | class RandomColor1 38 | { 39 | public: 40 | RandomColor1( ) { randColor( ); } 41 | 42 | void randColor( ) { color0 = rand( ) % 256; } 43 | cv::Scalar getColor( ) { return cv::Scalar( color0 ); } 44 | cv::Scalar getrandColor( ) 45 | { 46 | randColor( ); 47 | return cv::Scalar( color0 ); 48 | } 49 | 50 | private: 51 | int color0; 52 | }; 53 | } 54 | 55 | #endif // RANDOMCOLOR_H 56 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/cv_utils/scalartodata.h: -------------------------------------------------------------------------------- 1 | #ifndef SCALARTODATA_H 2 | #define SCALARTODATA_H 3 | 4 | #include 5 | 6 | namespace cv_utils 7 | { 8 | template< typename T > 9 | static inline void 10 | scalarToRawData_( const cv::Scalar& s, T* const buf, const int cn, const int unroll_to ) 11 | { 12 | int i = 0; 13 | for ( ; i < cn; i++ ) 14 | buf[i] = cv::saturate_cast< T >( s.val[i] ); 15 | for ( ; i < unroll_to; i++ ) 16 | buf[i] = buf[i - cn]; 17 | } 18 | 19 | inline void 20 | scalarToData( const cv::Scalar& s, void* _buf, int type, int unroll_to ) 21 | { 22 | const int depth = CV_MAT_DEPTH( type ), cn = CV_MAT_CN( type ); 23 | CV_Assert( cn <= 4 ); 24 | switch ( depth ) 25 | { 26 | case CV_8U: 27 | scalarToRawData_< uchar >( s, ( uchar* )_buf, cn, unroll_to ); 28 | break; 29 | case CV_8S: 30 | scalarToRawData_< schar >( s, ( schar* )_buf, cn, unroll_to ); 31 | break; 32 | case CV_16U: 33 | scalarToRawData_< ushort >( s, ( ushort* )_buf, cn, unroll_to ); 34 | break; 35 | case CV_16S: 36 | scalarToRawData_< short >( s, ( short* )_buf, cn, unroll_to ); 37 | break; 38 | case CV_32S: 39 | scalarToRawData_< int >( s, ( int* )_buf, cn, unroll_to ); 40 | break; 41 | case CV_32F: 42 | scalarToRawData_< float >( s, ( float* )_buf, cn, unroll_to ); 43 | break; 44 | case CV_64F: 45 | scalarToRawData_< double >( s, ( double* )_buf, cn, unroll_to ); 46 | break; 47 | default: 48 | CV_Error( CV_StsUnsupportedFormat, "" ); 49 | } 50 | } 51 | } 52 | 53 | #endif // SCALARTODATA_H 54 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/eigen_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_UTLIS_H 2 | #define EIGEN_UTLIS_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | using namespace std; 9 | 10 | namespace eigen_utils 11 | { 12 | 13 | typedef Eigen::Matrix Vector; 14 | typedef Eigen::Matrix 15 | Matrix; 16 | 17 | template 18 | struct EigenTypes 19 | { 20 | 21 | typedef Eigen::Matrix Matrix; 22 | 23 | typedef Eigen::Matrix Vector; 24 | }; 25 | 26 | /** 27 | * 28 | */ 29 | template 30 | inline vector_t 31 | SwapSequence(vector_t vec_in) 32 | { 33 | // int size = vec_in.size(); 34 | // vector_t vec_out(size); 35 | 36 | // for (int i = 0; i < size; i++) 37 | // { 38 | // vec_out(size - 1 - i) = vec_in(i); 39 | // } 40 | 41 | // return vec_out; 42 | 43 | return vec_in.reverse(); 44 | } 45 | 46 | inline Vector 47 | pushback(Vector vec_in, const double value) 48 | { 49 | Vector vec_out(vec_in.size() + 1); 50 | 51 | vec_out.segment(0, vec_in.size()) = vec_in; 52 | vec_out(vec_in.size()) = value; 53 | 54 | return vec_out; 55 | } 56 | 57 | template 58 | inline void 59 | copyMat3ToArry(const Eigen::Matrix mat, T* data) 60 | { 61 | data[0] = T(mat(0, 0)); 62 | data[1] = T(mat(1, 0)); 63 | data[2] = T(mat(2, 0)); 64 | data[3] = T(mat(0, 1)); 65 | data[4] = T(mat(1, 1)); 66 | data[5] = T(mat(2, 1)); 67 | data[6] = T(mat(0, 2)); 68 | data[7] = T(mat(1, 2)); 69 | data[8] = T(mat(2, 2)); 70 | } 71 | template 72 | inline void 73 | copyArryToMat3(T* data, Eigen::Matrix& mat) 74 | { 75 | mat(0, 0) = data[0]; 76 | mat(1, 0) = data[1]; 77 | mat(2, 0) = data[2]; 78 | mat(0, 1) = data[3]; 79 | mat(1, 1) = data[4]; 80 | mat(2, 1) = data[5]; 81 | mat(0, 2) = data[6]; 82 | mat(1, 2) = data[7]; 83 | mat(2, 2) = data[8]; 84 | } 85 | template 86 | inline void 87 | copyVector3ToArry(const Eigen::Matrix vec, T* data) 88 | { 89 | data[0] = T(vec(0)); 90 | data[1] = T(vec(1)); 91 | data[2] = T(vec(2)); 92 | } 93 | template 94 | inline void 95 | copyArryToVector3(T* data, Eigen::Matrix& vec) 96 | { 97 | vec(0) = data[0]; 98 | vec(1) = data[1]; 99 | vec(2) = data[2]; 100 | } 101 | 102 | }; 103 | 104 | #endif // EIGEN_UTLIS_H 105 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/math_utils/Polynomial.h: -------------------------------------------------------------------------------- 1 | #ifndef POLYNOMIAL_H 2 | #define POLYNOMIAL_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace math_utils 13 | { 14 | 15 | class Polynomial 16 | { 17 | 18 | public: 19 | Polynomial(); 20 | Polynomial(const int _order); 21 | Polynomial(const eigen_utils::Vector _coeff); 22 | 23 | eigen_utils::Vector getRealRoot(double _y); 24 | eigen_utils::Vector getRealRoot(double _y, double x_min, double x_max); 25 | double getOneRealRoot(double _y, double x_min, double x_max); 26 | 27 | eigen_utils::Vector getValue(const eigen_utils::Vector& in); 28 | double getValue(const double in); 29 | 30 | void setPolyOrder(int _order); 31 | int getPolyOrder() const; 32 | 33 | void setPolyCoeff(const eigen_utils::Vector& value); 34 | void setPolyCoeff(int order_index, double value); 35 | 36 | eigen_utils::Vector getPolyCoeff() const; 37 | double getPolyCoeff(int order_index) const; 38 | 39 | Polynomial& operator=(const Polynomial& other); 40 | friend std::ostream& operator<<(std::ostream& out, 41 | const Polynomial& poly); 42 | std::string toString(void) const; 43 | private: 44 | double Evaluate(double _x); 45 | 46 | // _y = f(x) 47 | bool FindRoots(const double y, const eigen_utils::Vector& polynomial_in, 48 | eigen_utils::Vector& real, eigen_utils::Vector& imaginary); 49 | 50 | void FindLinearPolynomialRoots(const eigen_utils::Vector& Polynomial, 51 | eigen_utils::Vector& real, 52 | eigen_utils::Vector& imag); 53 | 54 | void FindQuadraticPolynomialRoots(const eigen_utils::Vector& Polynomial, 55 | eigen_utils::Vector& real, 56 | eigen_utils::Vector& imag); 57 | 58 | void BuildCompanionMatrix(const eigen_utils::Vector& Polynomial, 59 | eigen_utils::Matrix* companion_matrix_ptr); 60 | 61 | // Balancing function as described by B. N. Parlett and C. Reinsch, 62 | // "Balancing a Matrix for Calculation of Eigenvalues and Eigenvectors". 63 | // In: Numerische Mathematik, Volume 13, Number 4 (1969), 293-304, 64 | // Springer Berlin / Heidelberg. DOI: 10.1007/BF02165404 65 | void BalanceCompanionMatrix(eigen_utils::Matrix* companion_matrix_ptr); 66 | 67 | private: 68 | int m_order; 69 | // c0, c1, c2, ..., cn 70 | // c0 + c1X + c2X^2 + ... +cnX^n 71 | eigen_utils::Vector m_coeff; 72 | }; 73 | 74 | typedef struct 75 | { 76 | double x; 77 | double y; 78 | } Sample; 79 | 80 | class PolynomialFit : public Polynomial 81 | { 82 | public: 83 | PolynomialFit(int _order); 84 | PolynomialFit(int _order, eigen_utils::Vector _x, eigen_utils::Vector _y); 85 | 86 | void loadSamples(const eigen_utils::Vector& x, 87 | const eigen_utils::Vector& y); 88 | void loadSample(const Sample sample); 89 | void clearSamples(); 90 | 91 | eigen_utils::Vector getCoeff(); 92 | Polynomial& getPolynomial(); 93 | 94 | private: 95 | eigen_utils::Vector Fit(); 96 | 97 | private: 98 | int data_size; 99 | vector samples; 100 | Polynomial* poly; 101 | eigen_utils::Vector coeff; 102 | }; 103 | 104 | } 105 | #endif // POLYNOMIAL_H 106 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/math_utils/acos_fast.h: -------------------------------------------------------------------------------- 1 | #ifndef ACOS_FAST_H 2 | #define ACOS_FAST_H 3 | 4 | template< typename T > 5 | T 6 | acosFaster_acc( const T x ) const 7 | { 8 | T a = 1.43 + 0.59 * x; 9 | a = ( a + ( 2 + 2 * x ) / a ) / 2; 10 | T b = 1.65 - 1.41 * x; 11 | b = ( b + ( 2 - 2 * x ) / b ) / 2; 12 | T c = 0.88 - 0.77 * x; 13 | c = ( c + ( 2 - a ) / c ) / 2; 14 | return ( 8 * ( c + ( 2 - a ) / c ) - ( b + ( 2 - 2 * x ) / b ) ) / 6; 15 | } 16 | 17 | template< typename T > 18 | T 19 | acosFaster_sqrt( const T x ) const 20 | { 21 | T a = sqrt( T( 2 ) + T( 2 ) * x ); 22 | T b = sqrt( T( 2 ) - T( 2 ) * x ); 23 | T c = sqrt( T( 2 ) - a ); 24 | return T( 8 ) / T( 3 ) * c - b / T( 3 ); 25 | } 26 | 27 | template< typename T > 28 | T 29 | acosFaster_linear( const T x ) const 30 | { 31 | return ( T( 3.14159 ) - T( 1.57079 ) * x ); 32 | } 33 | 34 | #endif // ACOS_FAST_H 35 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/math_utils/math_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef _MATH_UTILS_H 2 | #define _MATH_UTILS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #define DEG2RAD (M_PI/180.0) 9 | #define RAD2DEG (180.0/M_PI) 10 | 11 | #define DEG2RADF (float)(M_PI/180.0) 12 | #define RAD2DEGF (float)(180.0/M_PI) 13 | 14 | namespace math_utils 15 | { 16 | 17 | /// \brief Skew 18 | /// \param vec :input 3-dof vector 19 | /// \return :output skew symmetric matrix 20 | inline Eigen::Matrix3d 21 | vectorToSkew(const Eigen::Vector3d vec) 22 | { 23 | Eigen::Matrix3d mat; 24 | 25 | mat << 0.0, -vec(2), vec(1), 26 | vec(2), 0.0, -vec(0), 27 | -vec(1), vec(0), 0.0 ; 28 | 29 | return mat; 30 | } 31 | 32 | template 33 | inline Eigen::Matrix 34 | vectorToSkew(const Eigen::Matrix vec) 35 | { 36 | Eigen::Matrix mat; 37 | 38 | mat(0, 0) = T(0); 39 | mat(0, 1) = -vec(2); 40 | mat(0, 2) = vec(1); 41 | mat(1, 0) = vec(2); 42 | mat(1, 1) = T(0); 43 | mat(1, 2) = -vec(0); 44 | mat(2, 0) = -vec(1); 45 | mat(2, 1) = vec(0); 46 | mat(2, 2) = T(0); 47 | 48 | return mat; 49 | } 50 | 51 | /// \brief Skew 52 | /// \param vec :input 3-dof vector 53 | /// \return :output skew symmetric matrix 54 | inline Eigen::Vector3d 55 | skewToVector(const Eigen::Matrix3d mat) 56 | { 57 | return Eigen::Vector3d( mat(2,1), 58 | mat(0,2), 59 | mat(1,0) ); 60 | } 61 | 62 | /// \brief eigenQtoCeresQ 63 | /// \param q_eigen 64 | /// \return 65 | template 66 | inline T* 67 | eigenQtoCeresQ(const T* const q_eigen) 68 | { 69 | // Eigen convention (x, y, z, w) 70 | // Ceres convention (w, x, y, z) 71 | T q_ceres[4] = { q_eigen[3], 72 | q_eigen[0], 73 | q_eigen[1], 74 | q_eigen[2] }; 75 | 76 | return q_ceres; 77 | } 78 | 79 | /// \brief ceresQtoEigenQ 80 | /// \param q_ceres 81 | /// \return 82 | template 83 | inline T* 84 | ceresQtoEigenQ(const T* const q_ceres) 85 | { 86 | // Ceres convention (w, x, y, z) 87 | // Eigen convention (x, y, z, w) 88 | T q_eigen[4] = { q_ceres[1], 89 | q_ceres[2], 90 | q_ceres[3], 91 | q_ceres[0] }; 92 | 93 | return q_eigen; 94 | } 95 | 96 | /// \brief orientaionOfVectors 97 | /// \param v 98 | /// \param theta 99 | /// \return 100 | inline Eigen::Quaterniond 101 | orientaionOfVectors( const Eigen::Vector3d v, 102 | const double theta) 103 | { 104 | return Eigen::Quaterniond( cos(0.5 * theta), 105 | sin(0.5 * theta) * v(0), 106 | sin(0.5 * theta) * v(1), 107 | sin(0.5 * theta) * v(2) ); 108 | } 109 | 110 | /// \brief orientaionOfVectors :orientation betreen vector v2 to vetor v1 111 | /// \param v1 :begin vector 112 | /// \param v2 :end vector 113 | /// \return :q_12 114 | inline Eigen::Quaterniond 115 | orientaionOfVectors( const Eigen::Vector3d v1, 116 | const Eigen::Vector3d v2) 117 | { 118 | double theta = acos(v1.dot(v2)); 119 | 120 | Eigen::Vector3d Vk = v1.cross(v2); 121 | 122 | return Eigen::Quaterniond( cos(0.5 * theta), 123 | sin(0.5 * theta) * Vk(0), 124 | sin(0.5 * theta) * Vk(1), 125 | sin(0.5 * theta) * Vk(2) ); 126 | } 127 | 128 | inline Eigen::Matrix3d 129 | eularToDCM(double yaw, double pitch, double roll) 130 | { 131 | // double y = ypr(0) / 180.0 * M_PI; 132 | // double p = ypr(1) / 180.0 * M_PI; 133 | // double r = ypr(2) / 180.0 * M_PI; 134 | Eigen::Matrix3d Rz; 135 | Rz << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1; 136 | 137 | Eigen::Matrix3d Ry; 138 | Ry << cos(pitch), 0., sin(pitch), 0., 1., 0., -sin(pitch), 0., cos(pitch); 139 | 140 | Eigen::Matrix3d Rx; 141 | Rx << 1., 0., 0., 0., cos(roll), -sin(roll), 0., sin(roll), cos(roll); 142 | 143 | return Rz * Ry * Rx; 144 | } 145 | 146 | } 147 | #endif // _MATH_UTILS_H 148 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/sys_utils/cvmat_file_io.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CVMAT_FILE_IO_HPP 2 | #define CVMAT_FILE_IO_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace sys_utils 10 | { 11 | 12 | namespace io 13 | { 14 | 15 | // | C1 C2 C3 C4 16 | // CV_8U | uchar | 0 8 16 24 | -0 %8 17 | // CV_8S | char | 1 9 17 25 | -1 %8 18 | // CV_16U | | 2 10 18 26 | -2 %8 19 | // CV_16S | short | 3 11 19 27 | -3 %8 20 | // CV_32S | int | 4 12 20 28 | -4 %8 21 | // CV_32F | float | 5 13 21 29 | -5 %8 22 | // CV_64F | double | 6 14 22 30 | -6 %8 23 | 24 | inline void 25 | writeMatrixToBinary( const std::string filename, const cv::Mat& matrix ) 26 | { 27 | std::ofstream out( filename, std::ios::out | std::ios::binary | std::ios::trunc ); 28 | int rows = matrix.rows, cols = matrix.cols, step = matrix.step, type = matrix.type( ); 29 | 30 | out.write( ( char* )( &rows ), sizeof( int ) ); 31 | out.write( ( char* )( &cols ), sizeof( int ) ); 32 | out.write( ( char* )( &step ), sizeof( int ) ); 33 | out.write( ( char* )( &type ), sizeof( int ) ); 34 | 35 | // std::cout << " rows " << rows << "\n"; 36 | // std::cout << " cols " << cols << "\n"; 37 | // std::cout << " step " << step << "\n"; 38 | // std::cout << " type " << type << "\n"; 39 | 40 | out.write( ( char* )matrix.data, rows * step * sizeof( uchar ) ); 41 | 42 | out.close( ); 43 | } 44 | 45 | inline void 46 | parseMatrixFromBinary( const std::string filename, cv::Mat& matrix ) 47 | { 48 | std::ifstream in( filename, std::ios::in | std::ios::binary ); 49 | while ( in.peek( ) != EOF ) 50 | { 51 | int rows = 0, cols = 0, step = 0, type = 0; 52 | 53 | in.read( ( char* )( &rows ), sizeof( int ) ); 54 | in.read( ( char* )( &cols ), sizeof( int ) ); 55 | in.read( ( char* )( &step ), sizeof( int ) ); 56 | in.read( ( char* )( &type ), sizeof( int ) ); 57 | 58 | // std::cout << " rows " << rows << "\n"; 59 | // std::cout << " cols " << cols << "\n"; 60 | // std::cout << " step " << step << "\n"; 61 | // std::cout << " type " << type << "\n"; 62 | 63 | matrix = cv::Mat( rows, cols, type ); 64 | 65 | in.read( ( char* )matrix.data, rows * step * sizeof( uchar ) ); 66 | } 67 | 68 | in.close( ); 69 | } 70 | } 71 | } 72 | 73 | #endif // CVMAT_FILE_IO_HPP 74 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/sys_utils/eigen_file_io.hpp: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_FILE_IO_HPP 2 | #define EIGEN_FILE_IO_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace sys_utils 10 | { 11 | 12 | namespace io 13 | { 14 | 15 | template< class Matrix > 16 | void 17 | writeMatrixToBinary( const std::string filename, const Matrix& matrix ) 18 | { 19 | std::ofstream out( filename, std::ios::out | std::ios::binary | std::ios::trunc ); 20 | typename Matrix::Index rows = matrix.rows( ), cols = matrix.cols( ); 21 | out.write( ( char* )( &rows ), sizeof( typename Matrix::Index ) ); 22 | out.write( ( char* )( &cols ), sizeof( typename Matrix::Index ) ); 23 | out.write( ( char* )matrix.data( ), rows * cols * sizeof( typename Matrix::Scalar ) ); 24 | out.close( ); 25 | } 26 | 27 | template< class Matrix > 28 | void 29 | parseMatrixFromBinary( const std::string filename, Matrix& matrix ) 30 | { 31 | std::ifstream in( filename, std::ios::in | std::ios::binary ); 32 | while ( in.peek( ) != EOF ) 33 | { 34 | typename Matrix::Index rows = 0, cols = 0; 35 | in.read( ( char* )( &rows ), sizeof( typename Matrix::Index ) ); 36 | in.read( ( char* )( &cols ), sizeof( typename Matrix::Index ) ); 37 | matrix.resize( rows, cols ); 38 | in.read( ( char* )matrix.data( ), rows * cols * sizeof( typename Matrix::Scalar ) ); 39 | } 40 | in.close( ); 41 | } 42 | } 43 | } 44 | 45 | #endif // EIGEN_FILE_IO_HPP 46 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/sys_utils/float_equal.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FLOAT_EQUAL_HPP 2 | #define FLOAT_EQUAL_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace sys_utils 8 | { 9 | 10 | namespace equal 11 | { 12 | 13 | inline bool 14 | float_equal( const float a, const float b ) 15 | { 16 | if ( std::abs( a - b ) < 1e-6 ) 17 | return true; 18 | else 19 | return false; 20 | } 21 | 22 | inline bool 23 | double_equal( const double a, const double b ) 24 | { 25 | if ( std::abs( a - b ) < 1e-6 ) 26 | return true; 27 | else 28 | return false; 29 | } 30 | } 31 | } 32 | #endif // FLOAT_EQUAL_HPP 33 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/sys_utils/printcolor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PRINTCOLOR_HPP 2 | #define PRINTCOLOR_HPP 3 | 4 | #include 5 | 6 | namespace sys_utils 7 | { 8 | 9 | namespace print_color 10 | { 11 | 12 | inline void 13 | PrintWarning( std::string str ) 14 | { 15 | std::cout << "\033[33;40;1m" << str << "\033[0m" << std::endl; 16 | } 17 | 18 | inline void 19 | PrintError( std::string str ) 20 | { 21 | std::cout << "\033[31;47;1m" << str << "\033[0m" << std::endl; 22 | } 23 | 24 | inline void 25 | PrintInfo( std::string str ) 26 | { 27 | std::cout << "\033[32;40;1m" << str << "\033[0m" << std::endl; 28 | } 29 | } 30 | } 31 | #endif // PRINTCOLOR_HPP 32 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/sys_utils/tic_toc.h: -------------------------------------------------------------------------------- 1 | #ifndef TicToc_H 2 | #define TicToc_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace sys_utils 9 | { 10 | 11 | namespace tic 12 | { 13 | 14 | class TicToc 15 | { 16 | public: 17 | TicToc( ) { tic( ); } 18 | 19 | void tic( ) { start = std::chrono::system_clock::now( ); } 20 | 21 | double toc( ) 22 | { 23 | end = std::chrono::system_clock::now( ); 24 | 25 | std::chrono::duration< double > elapsed_seconds = end - start; 26 | return elapsed_seconds.count( ) * 1000; 27 | } 28 | 29 | private: 30 | std::chrono::time_point< std::chrono::system_clock > start, end; 31 | }; 32 | 33 | class TicTocPart 34 | { 35 | public: 36 | TicTocPart( ) { tic( ); } 37 | 38 | void tic( ) 39 | { 40 | start = std::chrono::system_clock::now( ); 41 | tmp = start; 42 | } 43 | 44 | double toc( ) 45 | { 46 | std::chrono::time_point< std::chrono::system_clock > now = std::chrono::system_clock::now( ); 47 | std::chrono::duration< double > elapsed_seconds = now - tmp; 48 | tmp = now; 49 | 50 | return elapsed_seconds.count( ) * 1000; 51 | } 52 | 53 | double tocEnd( ) 54 | { 55 | end = std::chrono::system_clock::now( ); 56 | 57 | std::chrono::duration< double > elapsed_seconds = end - start; 58 | return elapsed_seconds.count( ) * 1000; 59 | } 60 | 61 | private: 62 | std::chrono::time_point< std::chrono::system_clock > start, end, tmp; 63 | }; 64 | } 65 | } 66 | #endif // TicToc_H 67 | -------------------------------------------------------------------------------- /src/code_utils/include/code_utils/sys_utils/timeinseconds.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TIMEINSECONDS_HPP 2 | #define TIMEINSECONDS_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace sys_utils 8 | { 9 | 10 | inline unsigned long long 11 | timeInMicroseconds( void ) 12 | { 13 | struct timespec tp; 14 | 15 | clock_gettime( CLOCK_REALTIME, &tp ); 16 | 17 | return ( tp.tv_sec * 1000000 + tp.tv_nsec / 1000 ); 18 | } 19 | 20 | inline double 21 | timeInSeconds( void ) 22 | { 23 | struct timespec tp; 24 | 25 | clock_gettime( CLOCK_REALTIME, &tp ); 26 | 27 | return ( static_cast< double >( tp.tv_sec ) + static_cast< double >( tp.tv_nsec ) / 1000000000.0 ); 28 | } 29 | } 30 | #endif // TIMEINSECONDS_HPP 31 | -------------------------------------------------------------------------------- /src/code_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | code_utils 4 | 0.0.0 5 | The code_utils package 6 | 7 | 8 | 9 | 10 | gao 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | std_msgs 45 | roscpp 46 | std_msgs 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /src/code_utils/src/cv_utils.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | cv_utils::fisheye::PreProcess::PreProcess( const cv::Size _raw_image_size, 4 | const cv::Size _roi_size, 5 | const cv::Point _center, 6 | const float _resize_scale ) 7 | : is_preprocess( false ) 8 | { 9 | /* clang-format off */ 10 | if ( _raw_image_size.width >= _roi_size.width 11 | && _raw_image_size.height >= _roi_size.height 12 | && _center.x <= _raw_image_size.width 13 | && _center.y <= _raw_image_size.height 14 | && _roi_size.width != 0 15 | && _roi_size.height != 0 ) 16 | is_preprocess = true; 17 | else 18 | is_preprocess = false; 19 | /* clang-format on */ 20 | 21 | if ( is_preprocess ) 22 | resetPreProcess( _roi_size, _center, _resize_scale ); 23 | else 24 | std::cout << "[#ERROR] Parameters error." << std::endl; 25 | } 26 | 27 | void 28 | cv_utils::fisheye::PreProcess::resetPreProcess( cv::Size _roi_size, cv::Point _center, float _resize_scale ) 29 | { 30 | if ( _resize_scale < 0 ) 31 | resize_scale = 1.0; 32 | else 33 | resize_scale = _resize_scale; 34 | 35 | roi_row_start = _center.y - _roi_size.height / 2; 36 | roi_row_end = roi_row_start + _roi_size.height; 37 | roi_col_start = _center.x - _roi_size.width / 2; 38 | roi_col_end = roi_col_start + _roi_size.width; 39 | 40 | std::cout << "[#INFO] ROI row: start " << roi_row_start << " ,end " << roi_row_end << std::endl; 41 | std::cout << "[#INFO] ROI col: start " << roi_col_start << " ,end " << roi_col_end << std::endl; 42 | } 43 | 44 | cv::Mat 45 | cv_utils::fisheye::PreProcess::do_preprocess( cv::Mat image_input ) 46 | { 47 | if ( is_preprocess ) 48 | { 49 | cv::Mat image_input_roi = image_input( cv::Range( roi_row_start, roi_row_end ), 50 | cv::Range( roi_col_start, roi_col_end ) ); 51 | 52 | cv::Mat image_input_resized; 53 | cv::resize( image_input_roi, 54 | image_input_resized, 55 | cv::Size( image_input_roi.cols * resize_scale, image_input_roi.rows * resize_scale ) ); 56 | return image_input_resized; 57 | } 58 | else 59 | { 60 | return image_input; 61 | } 62 | } 63 | 64 | cv::Point2f 65 | cv_utils::fisheye::PreProcess::preprocessPoint( const cv::Point2f& pt_in ) 66 | { 67 | if ( is_preprocess ) 68 | { 69 | cv::Point2f pt( -1, -1 ); 70 | 71 | if ( !pt_in.inside( cv::Rect( roi_col_start, // 72 | roi_row_start, 73 | roi_col_end - roi_col_start, 74 | roi_row_end - roi_row_start ) ) ) 75 | { 76 | return pt; 77 | } 78 | else 79 | { 80 | pt = cv::Point2f( ( pt_in.x - roi_col_start ) * resize_scale, 81 | ( pt_in.y - roi_row_start ) * resize_scale ); 82 | return pt; 83 | } 84 | } 85 | else 86 | { 87 | return pt_in; 88 | } 89 | } 90 | -------------------------------------------------------------------------------- /src/code_utils/src/cv_utils/dlt/dlt.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | cv::DLT::DLT( const std::vector< Eigen::Vector3d >& pts_2, const std::vector< Eigen::Vector3d >& pts_3 ) 5 | { 6 | m_num_points = ( int )pts_2.size( ); 7 | 8 | if ( m_num_points != ( int )pts_3.size( ) ) 9 | std::cout << " [ERROR] input points size Error!" << std::endl; 10 | 11 | M.resize( 2 * m_num_points, 12 ); 12 | 13 | readPointsPlanar( pts_2, pts_3 ); 14 | 15 | solveDLT( ); 16 | } 17 | 18 | void 19 | cv::DLT::readPointsPlanar( const std::vector< Eigen::Vector3d >& pts_2, 20 | const std::vector< Eigen::Vector3d >& pts_3 ) 21 | { 22 | 23 | for ( int i = 0; i < m_num_points; i++ ) 24 | { 25 | 26 | int max_i = math_utils::max_index_in_three( pts_2[i].x( ), pts_2[i].y( ), pts_2[i].z( ) ); 27 | 28 | switch ( max_i ) 29 | { 30 | case 0: 31 | M( 2 * i, 0 ) = -pts_3[i].x( ) * pts_2[i].y( ) / pts_2[i].x( ); 32 | M( 2 * i, 1 ) = -pts_3[i].y( ) * pts_2[i].y( ) / pts_2[i].x( ); 33 | M( 2 * i, 2 ) = -pts_3[i].z( ) * pts_2[i].y( ) / pts_2[i].x( ); 34 | M( 2 * i, 3 ) = -pts_2[i].y( ) / pts_2[i].x( ); 35 | M( 2 * i, 4 ) = pts_3[i].x( ); 36 | M( 2 * i, 5 ) = pts_3[i].y( ); 37 | M( 2 * i, 6 ) = pts_3[i].z( ); 38 | M( 2 * i, 7 ) = 1; 39 | M( 2 * i, 8 ) = 0; 40 | M( 2 * i, 9 ) = 0; 41 | M( 2 * i, 10 ) = 0; 42 | M( 2 * i, 11 ) = 0; 43 | M( 2 * i + 1, 0 ) = -pts_3[i].x( ) * pts_2[i].z( ) / pts_2[i].x( ); 44 | M( 2 * i + 1, 1 ) = -pts_3[i].y( ) * pts_2[i].z( ) / pts_2[i].x( ); 45 | M( 2 * i + 1, 2 ) = -pts_3[i].z( ) * pts_2[i].z( ) / pts_2[i].x( ); 46 | M( 2 * i + 1, 3 ) = -pts_2[i].z( ) / pts_2[i].x( ); 47 | M( 2 * i + 1, 4 ) = 0; 48 | M( 2 * i + 1, 5 ) = 0; 49 | M( 2 * i + 1, 6 ) = 0; 50 | M( 2 * i + 1, 7 ) = 0; 51 | M( 2 * i + 1, 8 ) = pts_3[i].x( ); 52 | M( 2 * i + 1, 9 ) = pts_3[i].y( ); 53 | M( 2 * i + 1, 10 ) = pts_3[i].z( ); 54 | M( 2 * i + 1, 11 ) = 1; 55 | break; 56 | case 1: 57 | M( 2 * i, 0 ) = pts_3[i].x( ); 58 | M( 2 * i, 1 ) = pts_3[i].y( ); 59 | M( 2 * i, 2 ) = pts_3[i].z( ); 60 | M( 2 * i, 3 ) = 1; 61 | M( 2 * i, 4 ) = -pts_3[i].x( ) * pts_2[i].x( ) / pts_2[i].y( ); 62 | M( 2 * i, 5 ) = -pts_3[i].y( ) * pts_2[i].x( ) / pts_2[i].y( ); 63 | M( 2 * i, 6 ) = -pts_3[i].z( ) * pts_2[i].x( ) / pts_2[i].y( ); 64 | M( 2 * i, 7 ) = -pts_2[i].x( ) / pts_2[i].y( ); 65 | M( 2 * i, 8 ) = 0; 66 | M( 2 * i, 9 ) = 0; 67 | M( 2 * i, 10 ) = 0; 68 | M( 2 * i, 11 ) = 0; 69 | M( 2 * i + 1, 0 ) = 0; 70 | M( 2 * i + 1, 1 ) = 0; 71 | M( 2 * i + 1, 2 ) = 0; 72 | M( 2 * i + 1, 3 ) = 0; 73 | M( 2 * i + 1, 4 ) = -pts_3[i].x( ) * pts_2[i].z( ) / pts_2[i].y( ); 74 | M( 2 * i + 1, 5 ) = -pts_3[i].y( ) * pts_2[i].z( ) / pts_2[i].y( ); 75 | M( 2 * i + 1, 6 ) = -pts_3[i].z( ) * pts_2[i].z( ) / pts_2[i].y( ); 76 | M( 2 * i + 1, 7 ) = -pts_2[i].z( ) / pts_2[i].y( ); 77 | M( 2 * i + 1, 8 ) = pts_3[i].x( ); 78 | M( 2 * i + 1, 9 ) = pts_3[i].y( ); 79 | M( 2 * i + 1, 10 ) = pts_3[i].z( ); 80 | M( 2 * i + 1, 11 ) = 1; 81 | break; 82 | case 2: 83 | // Update matrix A using eq. 5 84 | M( 2 * i, 0 ) = pts_3[i].x( ); 85 | M( 2 * i, 1 ) = pts_3[i].y( ); 86 | M( 2 * i, 2 ) = pts_3[i].z( ); 87 | M( 2 * i, 3 ) = 1; 88 | M( 2 * i, 4 ) = 0; 89 | M( 2 * i, 5 ) = 0; 90 | M( 2 * i, 6 ) = 0; 91 | M( 2 * i, 7 ) = 0; 92 | M( 2 * i, 8 ) = -pts_2[i].x( ) / pts_2[i].z( ) * pts_3[i].x( ); 93 | M( 2 * i, 9 ) = -pts_2[i].x( ) / pts_2[i].z( ) * pts_3[i].y( ); 94 | M( 2 * i, 10 ) = -pts_2[i].x( ) / pts_2[i].z( ) * pts_3[i].z( ); 95 | M( 2 * i, 11 ) = -pts_2[i].x( ) / pts_2[i].z( ); 96 | M( 2 * i + 1, 0 ) = 0; 97 | M( 2 * i + 1, 1 ) = 0; 98 | M( 2 * i + 1, 2 ) = 0; 99 | M( 2 * i + 1, 3 ) = 0; 100 | M( 2 * i + 1, 4 ) = pts_3[i].x( ); 101 | M( 2 * i + 1, 5 ) = pts_3[i].y( ); 102 | M( 2 * i + 1, 6 ) = pts_3[i].z( ); 103 | M( 2 * i + 1, 7 ) = 1; 104 | M( 2 * i + 1, 8 ) = -pts_2[i].y( ) / pts_2[i].z( ) * pts_3[i].x( ); 105 | M( 2 * i + 1, 9 ) = -pts_2[i].y( ) / pts_2[i].z( ) * pts_3[i].y( ); 106 | M( 2 * i + 1, 10 ) = -pts_2[i].y( ) / pts_2[i].z( ) * pts_3[i].z( ); 107 | M( 2 * i + 1, 11 ) = -pts_2[i].y( ) / pts_2[i].z( ); 108 | break; 109 | } 110 | } 111 | } 112 | 113 | void 114 | cv::DLT::solveDLT( ) 115 | { 116 | // std::cout << " M " << std::endl << M << std::endl; 117 | 118 | Eigen::JacobiSVD< Eigen::MatrixXd > svd( M, Eigen::ComputeThinV ); 119 | // std::cout << " singularValues " << std::endl << svd.singularValues( ) << 120 | // std::endl; 121 | 122 | Eigen::MatrixXd h = svd.matrixV( ).block< 12, 1 >( 0, 11 ).transpose( ); 123 | 124 | // std::cout << " svd.matrixV( ) " << std::endl << svd.matrixV( ) << std::endl; 125 | 126 | for ( int i = 0; i < 3; i++ ) 127 | { 128 | T( i ) = h( 0, 4 * i + 3 ); // Translation 129 | 130 | for ( int j = 0; j < 3; j++ ) 131 | RR( i, j ) = h( 0, 4 * i + j ); // Rotation 132 | } 133 | 134 | // std::cout << " h " << std::endl << h << std::endl; 135 | double norm = sqrt( h( 0, 8 ) * h( 0, 8 ) + h( 0, 9 ) * h( 0, 9 ) + h( 0, 10 ) * h( 0, 10 ) ); 136 | 137 | if ( RR.determinant( ) < 0 ) // tz < 0 138 | { 139 | T /= -norm; 140 | RR = RR / -norm; 141 | } 142 | else 143 | { 144 | T /= norm; 145 | RR = RR / norm; 146 | } 147 | 148 | Eigen::JacobiSVD< Eigen::MatrixXd > svd2( RR, Eigen::ComputeThinU | Eigen::ComputeThinV ); 149 | 150 | R = svd2.matrixU( ) * svd2.matrixV( ).transpose( ); 151 | } 152 | -------------------------------------------------------------------------------- /src/code_utils/src/cv_utils/pnp/linearpnp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | cv::Homography::Homography( const std::vector< Eigen::Vector3d >& pts_2, 4 | const std::vector< Eigen::Vector3d >& pts_3 ) 5 | { 6 | mat_tmp.resize( 2, 9 ); 7 | 8 | readPointsPlanar( pts_2, pts_3 ); 9 | 10 | solvePnP( ); 11 | } 12 | 13 | void 14 | cv::Homography::solvePnP( ) 15 | { 16 | // std::cout << "Here is the matrix m:" << std::endl << M << std::endl; 17 | Eigen::JacobiSVD< Eigen::MatrixXd > svd( M, Eigen::ComputeThinV ); 18 | // std::cout << "Its singular values are:" << std::endl << svd.singularValues( ) << 19 | // std::endl; 20 | // std::cout << "svd.matrixV( )" << std::endl << svd.matrixV( ) << std::endl; 21 | 22 | // the last column of matrixV 23 | Eigen::Matrix3d hat_H; 24 | 25 | hat_H.block< 1, 3 >( 0, 0 ) = svd.matrixV( ).block< 3, 1 >( 0, 8 ).transpose( ); 26 | hat_H.block< 1, 3 >( 1, 0 ) = svd.matrixV( ).block< 3, 1 >( 3, 8 ).transpose( ); 27 | hat_H.block< 1, 3 >( 2, 0 ) = svd.matrixV( ).block< 3, 1 >( 6, 8 ).transpose( ); 28 | 29 | std::cout << hat_H << std::endl; 30 | 31 | Eigen::Matrix3d RRT = hat_H; 32 | 33 | T( 0 ) = RRT.coeff( 0, 2 ); 34 | T( 1 ) = RRT.coeff( 1, 2 ); 35 | T( 2 ) = RRT.coeff( 2, 2 ); 36 | 37 | Eigen::Vector3d h1 = RRT.block< 3, 1 >( 0, 0 ); 38 | Eigen::Vector3d h2 = RRT.block< 3, 1 >( 0, 1 ); 39 | Eigen::Vector3d h3 = h1.cross( h2 ); 40 | 41 | if ( RRT.determinant( ) < 0 ) 42 | { 43 | h1 = -h1; 44 | h2 = -h2; 45 | T = -T; 46 | } 47 | 48 | Eigen::Matrix3d hat_H_2; 49 | hat_H_2 << h1, h2, h3; 50 | 51 | Eigen::JacobiSVD< Eigen::MatrixXd > svd2( hat_H_2, Eigen::ComputeThinU | Eigen::ComputeThinV ); 52 | 53 | R = svd2.matrixU( ) * svd2.matrixV( ).transpose( ); 54 | // std::cout << "R " << std::endl << R << std::endl; 55 | T = T / ( h1.norm( ) ); 56 | } 57 | 58 | void 59 | cv::Homography::readPointsPlanar( const std::vector< Eigen::Vector3d >& pts_2, 60 | const std::vector< Eigen::Vector3d >& pts_3 ) 61 | { 62 | int index_max = pts_3.size( ); 63 | // std::cout << " size " << index_max << std::endl; 64 | M.resize( 2 * index_max, 9 ); 65 | // M.setZero( ); 66 | 67 | int max_i; 68 | 69 | for ( int index = 0; index < index_max; index++ ) 70 | { 71 | // std::cout << pts_3.at( index ).x( ) << " " << pts_3.at( index ).y( ) << " " 72 | // << pts_3.at( index ).z( ) << std::endl; 73 | // std::cout << pts_2.at( index ).x( ) << " " << pts_2.at( index ).y( ) << " " 74 | // << pts_2.at( index ).z( ) << std::endl; 75 | 76 | max_i = math_utils::max_index_in_three( pts_2[index].x( ), // 77 | pts_2[index].y( ), 78 | pts_2[index].z( ) ); 79 | 80 | switch ( max_i ) 81 | { 82 | case 0: 83 | // std::cout << " x " << std::endl; 84 | mat_tmp << -pts_3[index].x( ) * pts_2[index].y( ) / pts_2[index].x( ), 85 | -pts_3[index].y( ) * pts_2[index].y( ) / pts_2[index].x( ), 86 | -pts_2[index].y( ) / pts_2[index].x( ), pts_3[index].x( ), pts_3[index].y( ), 87 | 1, 0, 0, 0, -pts_3[index].x( ) * pts_2[index].z( ) / pts_2[index].x( ), 88 | -pts_3[index].y( ) * pts_2[index].z( ) / pts_2[index].x( ), 89 | -pts_2[index].z( ) / pts_2[index].x( ), 0, 0, 0, pts_3[index].x( ), 90 | pts_3[index].y( ), 1; 91 | break; 92 | case 1: 93 | // std::cout << " y " << std::endl; 94 | mat_tmp << pts_3[index].x( ), pts_3[index].y( ), 1, 95 | -pts_3[index].x( ) * pts_2[index].x( ) / pts_2[index].y( ), 96 | -pts_3[index].y( ) * pts_2[index].x( ) / pts_2[index].y( ), 97 | -pts_2[index].x( ) / pts_2[index].y( ), 0, 0, 0, 0, 0, 0, 98 | -pts_3[index].x( ) * pts_2[index].z( ) / pts_2[index].y( ), 99 | -pts_3[index].y( ) * pts_2[index].z( ) / pts_2[index].y( ), 100 | -pts_2[index].z( ) / pts_2[index].y( ), pts_3[index].x( ), pts_3[index].y( ), 1; 101 | break; 102 | case 2: 103 | // std::cout << " z " << std::endl; 104 | mat_tmp << pts_3[index].x( ), pts_3[index].y( ), 1, 0, 0, 0, 105 | -pts_3[index].x( ) * pts_2[index].x( ) / pts_2[index].z( ), 106 | -pts_3[index].y( ) * pts_2[index].x( ) / pts_2[index].z( ), 107 | -pts_2[index].x( ) / pts_2[index].z( ), 0, 0, 0, pts_3[index].x( ), 108 | pts_3[index].y( ), 1, -pts_3[index].x( ) * pts_2[index].y( ) / pts_2[index].z( ), 109 | -pts_3[index].y( ) * pts_2[index].y( ) / pts_2[index].z( ), 110 | -pts_2[index].y( ) / pts_2[index].z( ); 111 | break; 112 | default: 113 | // std::cout << " default z " << std::endl; 114 | mat_tmp << pts_3[index].x( ), pts_3[index].y( ), 1, 0, 0, 0, 115 | -pts_3[index].x( ) * pts_2[index].x( ) / pts_2[index].z( ), 116 | -pts_3[index].y( ) * pts_2[index].x( ) / pts_2[index].z( ), 117 | -pts_2[index].x( ) / pts_2[index].z( ), 0, 0, 0, pts_3[index].x( ), 118 | pts_3[index].y( ), 1, -pts_3[index].x( ) * pts_2[index].y( ) / pts_2[index].z( ), 119 | -pts_3[index].y( ) * pts_2[index].y( ) / pts_2[index].z( ), 120 | -pts_2[index].y( ) / pts_2[index].z( ); 121 | break; 122 | } 123 | // std::cout << " mat_tmp " << std::endl << mat_tmp << std::endl; 124 | 125 | M.block< 2, 9 >( 2 * index, 0 ) = mat_tmp; 126 | } 127 | // std::cout << " M " << std::endl << M << std::endl; 128 | } 129 | -------------------------------------------------------------------------------- /src/code_utils/src/cv_utils/pnp/nonlinearpnp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | cv::NonlinearPnP::NonlinearPnP( const Eigen::Matrix3d& _R_initial, 4 | const Eigen::Vector3d& _T_initial, 5 | const std::vector< Eigen::Vector3d >& image_point, 6 | const std::vector< Eigen::Vector3d >& scene_point ) 7 | : T( _T_initial ) 8 | { 9 | 10 | cv::ProjectionFactor::sqrt_info = Eigen::Matrix2d::Identity( ); 11 | 12 | if ( image_point.size( ) != scene_point.size( ) ) 13 | std::cerr << "Error of point size" << std::endl; 14 | q = _R_initial; 15 | 16 | // initialize the params to something close to the gt 17 | double ext[] = { T[0], T[1], T[2], q.x( ), q.y( ), q.z( ), q.w( ) }; 18 | 19 | point_num = image_point.size( ); 20 | 21 | ceres::Problem problem; 22 | 23 | for ( int i = 0; i < point_num; ++i ) 24 | { 25 | 26 | // ProjectionFactor* f = new ProjectionFactor( image_point[i], scene_point[i] ); 27 | 28 | ceres::CostFunction* f = new ceres::AutoDiffCostFunction< ReprojectionError, 2, 7 >( 29 | new ReprojectionError( image_point[i], scene_point[i] ) ); 30 | 31 | problem.AddResidualBlock( f, NULL /* squared loss */, ext ); 32 | } 33 | 34 | ceres::Solver::Options options; 35 | options.minimizer_progress_to_stdout = true; 36 | options.logging_type = ceres::SILENT; 37 | options.trust_region_strategy_type = ceres::DOGLEG; 38 | // options.max_num_iterations = 5; 39 | 40 | ceres::Solver::Summary summary; 41 | ceres::Solve( options, &problem, &summary ); 42 | // std::cout << summary.FullReport( ) << "\n"; 43 | 44 | q.w( ) = ext[6]; 45 | q.x( ) = ext[3]; 46 | q.y( ) = ext[4]; 47 | q.z( ) = ext[5]; 48 | T << ext[0], ext[1], ext[2]; 49 | 50 | if ( summary.final_cost > 1 ) 51 | solved = false; 52 | else 53 | solved = true; 54 | } 55 | 56 | bool 57 | cv::NonlinearPnP::getRT( Eigen::Vector3d& T_out, Eigen::Quaterniond& q_out ) 58 | { 59 | q_out = q.normalized( ); 60 | T_out = T; 61 | 62 | if ( solved ) 63 | return true; 64 | else 65 | return false; 66 | } 67 | 68 | cv::NonlinearPnP::ReprojectionError::ReprojectionError( const Eigen::Vector3d& image_point, 69 | const Eigen::Vector3d& scene_point ) 70 | : image_point_( image_point ) 71 | , scene_point_( scene_point ) 72 | { 73 | Eigen::Vector3d b1, b2; 74 | Eigen::Vector3d tmp( 0, 0, 1 ); 75 | Eigen::Vector3d a = scene_point_.normalized( ); 76 | if ( a == tmp ) 77 | tmp << 1, 0, 0; 78 | 79 | b1 = ( tmp - a * ( a.transpose( ) * tmp ) ).normalized( ); 80 | b2 = a.cross( b1 ); 81 | 82 | tangent_base.block< 1, 3 >( 0, 0 ) = b1.transpose( ); 83 | tangent_base.block< 1, 3 >( 1, 0 ) = b2.transpose( ); 84 | } 85 | 86 | cv::ProjectionFactor::ProjectionFactor( const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j ) 87 | : image_point_( _pts_i ) 88 | , scene_point_( _pts_j ) 89 | { 90 | Eigen::Vector3d b1, b2; 91 | Eigen::Vector3d a = scene_point_.normalized( ); 92 | Eigen::Vector3d tmp( 0, 0, 1 ); 93 | if ( a == tmp ) 94 | tmp << 1, 0, 0; 95 | b1 = ( tmp - a * ( a.transpose( ) * tmp ) ).normalized( ); 96 | b2 = a.cross( b1 ); 97 | tangent_base.block< 1, 3 >( 0, 0 ) = b1.transpose( ); 98 | tangent_base.block< 1, 3 >( 1, 0 ) = b2.transpose( ); 99 | } 100 | 101 | Eigen::Matrix2d cv::ProjectionFactor::sqrt_info; 102 | 103 | bool 104 | cv::ProjectionFactor::Evaluate( const double* const* ex_paramt, double* residuals, double** jacobians ) const 105 | { 106 | 107 | Eigen::Vector3d _t_c( ex_paramt[0][0], ex_paramt[0][1], ex_paramt[0][2] ); 108 | Eigen::Quaterniond _q_cw( ex_paramt[0][6], ex_paramt[0][3], ex_paramt[0][4], ex_paramt[0][5] ); 109 | // _q_cw.normalize( ); 110 | 111 | Eigen::Matrix< double, 3, 1 > p_c; 112 | p_c = _q_cw * scene_point_ + _t_c; 113 | 114 | Eigen::Map< Eigen::Vector2d > residual( residuals ); 115 | 116 | residual = tangent_base * ( p_c.normalized( ) - image_point_ ); 117 | // residual = sqrt_info * residual; 118 | 119 | if ( jacobians ) 120 | { 121 | Eigen::Matrix3d R_cw = _q_cw.toRotationMatrix( ); 122 | Eigen::Matrix< double, 2, 3 > reduce( 2, 3 ); 123 | 124 | double norm = p_c.norm( ); 125 | Eigen::Matrix3d norm_jaco; 126 | double x1, x2, x3; 127 | x1 = p_c( 0 ); 128 | x2 = p_c( 1 ); 129 | x3 = p_c( 2 ); 130 | 131 | // clang-format off 132 | norm_jaco << 133 | 1.0 / norm - x1 * x1 / pow( norm, 3 ), -x1 * x2 / pow( norm, 3 ), -x1 * x3 / pow( norm, 3 ), 134 | -x1 * x2 / pow( norm, 3 ), 1.0 / norm - x2 * x2 / pow( norm, 3 ), -x2 * x3 / pow( norm, 3 ), 135 | -x1 * x3 / pow( norm, 3 ), -x2 * x3 / pow( norm, 3 ), 1.0 / norm - x3 * x3 / pow( norm, 3 ); 136 | // clang-format on 137 | 138 | if ( jacobians[0] ) 139 | { 140 | Eigen::Map< Eigen::Matrix< double, 2, 7, Eigen::RowMajor > > jacobian_pose_i( jacobians[0] ); 141 | // clang-format on 142 | Eigen::Matrix< double, 3, 6 > jaco_i; 143 | jaco_i.leftCols< 3 >( ) = Eigen::Matrix3d::Identity( ); 144 | jaco_i.rightCols< 3 >( ) = -R_cw * Utility::skewSymmetric( scene_point_ ); 145 | // clang-format on 146 | 147 | jacobian_pose_i.leftCols< 6 >( ) = tangent_base * jaco_i; 148 | jacobian_pose_i.rightCols< 1 >( ).setZero( ); 149 | } 150 | } 151 | return true; 152 | } 153 | -------------------------------------------------------------------------------- /src/code_utils/src/cv_utils/pnp/pnp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | cv::Pnp::Pnp( const std::vector< Eigen::Vector3d >& image_point, 4 | const std::vector< Eigen::Vector3d >& scene_point ) 5 | : solved( false ) 6 | { 7 | // std::cout << "image_point " << image_point.size( ) << std::endl; 8 | // std::cout << "scene_point " << scene_point.size( ) << std::endl; 9 | 10 | bool is_planar = false; 11 | if ( scene_point.size( ) < 6 ) 12 | { 13 | is_planar = true; 14 | } 15 | else 16 | { 17 | int size = scene_point.size( ); 18 | Eigen::MatrixXd scene_points( size, 3 ); 19 | for ( int index = 0; index < size; ++index ) 20 | { 21 | scene_points( index, 0 ) = scene_point[index]( 0 ); 22 | scene_points( index, 1 ) = scene_point[index]( 1 ); 23 | scene_points( index, 2 ) = scene_point[index]( 2 ); 24 | } 25 | Eigen::MatrixXd scene_points2 = scene_points * scene_points.transpose( ); 26 | 27 | Eigen::VectorXd svd_si = Eigen::JacobiSVD< Eigen::MatrixXd >( scene_points2, // 28 | Eigen::EigenvaluesOnly ) 29 | .singularValues( ); 30 | 31 | // Eigen::Vector3d svd_si = svd.singularValues( ); 32 | // std::cout << svd_si << std::endl; 33 | // std::cout << "is_planar ? " << svd_si( 2 ) / svd_si( 1 ) << std::endl; 34 | 35 | if ( ( svd_si( 2 ) / svd_si( 1 ) ) < 0.001 ) 36 | is_planar = true; 37 | else 38 | is_planar = false; 39 | } 40 | // std::cout << "is_planar " << is_planar << std::endl; 41 | 42 | Eigen::Matrix3d R_cw; 43 | Eigen::Vector3d t_cw; 44 | 45 | if ( is_planar ) 46 | { 47 | cv::Homography llpnp( image_point, scene_point ); 48 | R_cw = llpnp.getR( ); 49 | t_cw = llpnp.getT( ); 50 | } 51 | else 52 | { 53 | cv::DLT llpnp( image_point, scene_point ); 54 | R_cw = llpnp.getR( ); 55 | t_cw = llpnp.getT( ); 56 | } 57 | 58 | // std::cout << "R_cw " << std::endl << R_cw << std::endl; 59 | // std::cout << "t_cw " << std::endl << t_cw.transpose( ) << std::endl; 60 | 61 | npnp = new cv::NonlinearPnP( R_cw, t_cw, image_point, scene_point ); 62 | } 63 | 64 | cv::Pnp::Pnp( const std::vector< Eigen::Vector3d >& image_point, 65 | const std::vector< Eigen::Vector3d >& scene_point, 66 | Eigen::Quaterniond& q_dst, 67 | Eigen::Vector3d& T_dst ) 68 | : solved( false ) 69 | { 70 | bool is_planar = false; 71 | if ( scene_point.size( ) < 6 ) 72 | { 73 | is_planar = true; 74 | } 75 | else 76 | { 77 | int size = scene_point.size( ); 78 | Eigen::MatrixXd scene_points( size, 3 ); 79 | for ( int index = 0; index < size; ++index ) 80 | { 81 | scene_points( index, 0 ) = scene_point[index]( 0 ); 82 | scene_points( index, 1 ) = scene_point[index]( 1 ); 83 | scene_points( index, 2 ) = scene_point[index]( 2 ); 84 | } 85 | Eigen::MatrixXd scene_points2 = scene_points * scene_points.transpose( ); 86 | 87 | Eigen::JacobiSVD< Eigen::MatrixXd > svd( scene_points2, Eigen::EigenvaluesOnly ); 88 | 89 | // std::cout << svd.singularValues( ) << std::endl; 90 | 91 | if ( svd.singularValues( )( 2 ) / svd.singularValues( )( 1 ) < 1e-3 ) 92 | is_planar = true; 93 | else 94 | is_planar = false; 95 | } 96 | 97 | Eigen::Matrix3d R_cw; 98 | Eigen::Vector3d t_cw; 99 | if ( is_planar ) 100 | { 101 | cv::Homography llpnp( image_point, scene_point ); 102 | R_cw = llpnp.getR( ); 103 | t_cw = llpnp.getT( ); 104 | } 105 | else 106 | { 107 | cv::DLT llpnp( image_point, scene_point ); 108 | R_cw = llpnp.getR( ); 109 | t_cw = llpnp.getT( ); 110 | } 111 | 112 | // q_dst = Eigen::Quaterniond( R_cw ); 113 | // T_dst = t_cw; 114 | 115 | // std::cout << "P_2 " << std::endl << t_cw.transpose( ) << std::endl; 116 | 117 | npnp = new cv::NonlinearPnP( R_cw, t_cw, image_point, scene_point ); 118 | getRT( q_dst, T_dst ); 119 | } 120 | 121 | cv::Pnp::Pnp( const std::vector< Eigen::Vector3d >& image_point, 122 | const std::vector< Eigen::Vector3d >& scene_point, 123 | const Eigen::Quaterniond& q_init, 124 | const Eigen::Vector3d& T_init, 125 | Eigen::Quaterniond& q_dst, 126 | Eigen::Vector3d& T_dst ) 127 | : solved( false ) 128 | { 129 | npnp = new cv::NonlinearPnP( q_init.toRotationMatrix( ), T_init, image_point, scene_point ); 130 | getRT( q_dst, T_dst ); 131 | } 132 | 133 | bool 134 | cv::Pnp::getRT( Eigen::Quaterniond& q_dst, Eigen::Vector3d& T_dst ) 135 | { 136 | return npnp->getRT( T_dst, q_dst ); 137 | } 138 | -------------------------------------------------------------------------------- /src/code_utils/src/mat_io_test.cpp: -------------------------------------------------------------------------------- 1 | #define BACKWARD_HAS_DW 1 2 | #include "code_utils/backward.hpp" 3 | namespace backward 4 | { 5 | backward::SignalHandling sh; 6 | } 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace std; 15 | using namespace cv; 16 | 17 | double t; 18 | 19 | void 20 | showImg( std::string name, cv::Mat img ) 21 | { 22 | cv::namedWindow( name, WINDOW_NORMAL ); 23 | cv::imshow( name, img ); 24 | } 25 | 26 | int 27 | main( ) 28 | { 29 | Mat img; 30 | 31 | // Mat img1 = imread( "/home/gao/ws/devel/lib/camera_model/image_down/IMG_35.png", 32 | // CV_LOAD_IMAGE_GRAYSCALE ); 33 | Mat img1 = imread( "/home/gao/IMG_1.png", CV_LOAD_IMAGE_UNCHANGED ); 34 | 35 | cv::resize( img1, img, cv::Size( 640, 512 ) ); 36 | 37 | sys_utils::io::writeMatrixToBinary( "/home/gao/img_data", img1 ); 38 | 39 | cv::Mat img11; 40 | sys_utils::io::parseMatrixFromBinary( "/home/gao/img_data", img11 ); 41 | showImg( "img_data", img11 ); 42 | 43 | cv::Mat img2( 2, 2, CV_8UC3 ); 44 | for ( int i = 0; i < 3; ++i ) 45 | { 46 | img2.at< cv::Vec12b >( 0, 0 )[i] = uchar( 1 ); 47 | img2.at< cv::Vec12b >( 0, 1 )[i] = uchar( 2 ); 48 | img2.at< cv::Vec12b >( 1, 0 )[i] = uchar( 3 ); 49 | img2.at< cv::Vec12b >( 1, 1 )[i] = uchar( 4 ); 50 | } 51 | std::cout << img2 << "\n\n"; 52 | sys_utils::io::writeMatrixToBinary( "/home/gao/img_data2", img2 ); 53 | 54 | cv::Mat img3( 2, 2, CV_32FC3 ); 55 | for ( int i = 0; i < 3; ++i ) 56 | { 57 | img3.at< cv::Vec3f >( 0, 0 )[i] = float( 1.1 ); 58 | img3.at< cv::Vec3f >( 0, 1 )[i] = float( 2.2 ); 59 | img3.at< cv::Vec3f >( 1, 0 )[i] = float( 3.3 ); 60 | img3.at< cv::Vec3f >( 1, 1 )[i] = float( 4.4 ); 61 | } 62 | std::cout << img3 << "\n\n"; 63 | sys_utils::io::writeMatrixToBinary( "/home/gao/img_data3", img3 ); 64 | 65 | cv::Mat img4( 2, 2, CV_64FC3 ); 66 | for ( int i = 0; i < 3; ++i ) 67 | { 68 | img4.at< cv::Vec3d >( 0, 0 )[i] = float( 1.11 ); 69 | img4.at< cv::Vec3d >( 0, 1 )[i] = float( 2.22 ); 70 | img4.at< cv::Vec3d >( 1, 0 )[i] = float( 3.33 ); 71 | img4.at< cv::Vec3d >( 1, 1 )[i] = float( 4.44 ); 72 | } 73 | std::cout << img4 << "\n\n"; 74 | sys_utils::io::writeMatrixToBinary( "/home/gao/img_data4", img4 ); 75 | 76 | std::cout << "====================================" 77 | << "\n\n"; 78 | 79 | cv::Mat img22; 80 | sys_utils::io::parseMatrixFromBinary( "/home/gao/img_data2", img22 ); 81 | std::cout << img22 << "\n\n"; 82 | 83 | cv::Mat img33; 84 | sys_utils::io::parseMatrixFromBinary( "/home/gao/img_data3", img33 ); 85 | std::cout << img33 << "\n\n"; 86 | 87 | cv::Mat img44; 88 | sys_utils::io::parseMatrixFromBinary( "/home/gao/img_data4", img44 ); 89 | std::cout << img44 << "\n\n"; 90 | 91 | waitKey( 0 ); 92 | 93 | return 0; 94 | } 95 | -------------------------------------------------------------------------------- /src/code_utils/src/poly_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | using namespace Eigen; 10 | 11 | void 12 | fit_test( ) 13 | { 14 | eigen_utils::Vector xx( 101 ); 15 | eigen_utils::Vector out( 101 ); 16 | 17 | xx << 0, 0.0261799, 0.0523599, 0.0785398, 0.10472, 0.1309, 0.15708, 0.18326, 0.20944, 18 | 0.235619, 0.261799, 0.287979, 0.314159, 0.340339, 0.366519, 0.392699, 0.418879, 19 | 0.445059, 0.471239, 0.497419, 0.523599, 0.549779, 0.575959, 0.602139, 0.628319, 20 | 0.654498, 0.680678, 0.706858, 0.733038, 0.759218, 0.785398, 0.811578, 0.837758, 21 | 0.863938, 0.890118, 0.916298, 0.942478, 0.968658, 0.994838, 1.02102, 1.0472, 1.07338, 22 | 1.09956, 1.12574, 1.15192, 1.1781, 1.20428, 1.23046, 1.25664, 1.28282, 1.309, 1.33518, 23 | 1.36136, 1.38754, 1.41372, 1.4399, 1.46608, 1.49226, 1.51844, 1.54462, 1.5708, 1.59698, 24 | 1.62316, 1.64934, 1.67552, 1.7017, 1.72788, 1.75406, 1.78024, 1.80642, 1.8326, 1.85878, 25 | 1.88496, 1.91114, 1.93732, 1.9635, 1.98968, 2.01586, 2.04204, 2.06822, 2.0944, 2.12058, 26 | 2.14675, 2.17293, 2.19911, 2.22529, 2.25147, 2.27765, 2.30383, 2.33001, 2.35619, 27 | 2.38237, 2.40855, 2.43473, 2.46091, 2.48709, 2.51327, 2.53945, 2.56563, 2.59181, 2.61799; 28 | 29 | out << 0, 0.0261771, 0.0523467, 0.0785066, 0.104654, 0.130789, 0.156907, 0.183008, 30 | 0.209091, 0.235154, 0.261196, 0.287216, 0.313212, 0.339184, 0.365131, 0.391052, 31 | 0.416945, 0.44281, 0.468646, 0.494452, 0.520226, 0.545968, 0.571676, 0.597349, 0.622986, 32 | 0.648584, 0.674144, 0.699662, 0.725137, 0.750567, 0.775951, 0.801285, 0.826568, 0.851797, 33 | 0.876968, 0.90208, 0.927128, 0.95211, 0.977021, 1.00186, 1.02661, 1.05129, 1.07587, 34 | 1.10036, 1.12475, 1.14903, 1.17319, 1.19724, 1.22115, 1.24492, 1.26855, 1.29201, 1.3153, 35 | 1.33841, 1.36132, 1.38402, 1.4065, 1.42873, 1.4507, 1.4724, 1.49379, 1.51486, 1.53558, 36 | 1.55594, 1.57589, 1.59541, 1.61448, 1.63305, 1.65109, 1.66857, 1.68543, 1.70165, 37 | 1.71716, 1.73193, 1.7459, 1.759, 1.77119, 1.7824, 1.79256, 1.80159, 1.80943, 1.81599, 38 | 1.82119, 1.82494, 1.82713, 1.82767, 1.82646, 1.82338, 1.81831, 1.81112, 1.80168, 39 | 1.78986, 1.77551, 1.75847, 1.73857, 1.71566, 1.68955, 1.66005, 1.62696, 1.59008, 1.54919; 40 | 41 | math_utils::PolynomialFit polyfit( 24, out, xx ); 42 | math_utils::Polynomial poly = polyfit.getCoeff( ); 43 | std::cout << "polyfit :" << endl << poly.getPolyCoeff( ).transpose( ) << std::endl; 44 | 45 | double dd = 0.02; 46 | for ( int i = 0; i < 100; ++i ) 47 | { 48 | std::cout << dd * i << " " << poly.getValue( dd * i ) << std::endl; 49 | } 50 | } 51 | 52 | void 53 | test_poly( ) 54 | { 55 | eigen_utils::Vector coeff( 6 ); 56 | coeff << 1, 2, 3, 4, 5, 6; 57 | 58 | std::cout << "coeff :" << endl << coeff.transpose( ) << std::endl; 59 | 60 | math_utils::Polynomial poly( 5 ); 61 | poly.setPolyCoeff( coeff ); 62 | std::cout << "coeff :" << endl << poly.getPolyCoeff( ).transpose( ) << std::endl; 63 | 64 | for ( int i = 0; i < 10; i++ ) 65 | { 66 | std::cout << "x: " << i << " y : " << poly.getValue( i ) << std::endl; 67 | } 68 | 69 | eigen_utils::Vector xx = coeff; 70 | eigen_utils::Vector out = poly.getValue( xx ); 71 | std::cout << "out :" << endl << out.transpose( ) << std::endl; 72 | 73 | std::cout << ":--PolynomialFit--------------------------------------:" << std::endl; 74 | math_utils::PolynomialFit polyfit( 5, xx, out ); 75 | std::cout << "polyfit :" << endl << polyfit.getCoeff( ).transpose( ) << std::endl; 76 | 77 | math_utils::Polynomial polyfited; 78 | polyfited = polyfit.getPolynomial( ); 79 | std::cout << "polyfited :" << endl 80 | << polyfited.getPolyCoeff( ).transpose( ) << std::endl; 81 | 82 | math_utils::Polynomial* polyfited2 = new math_utils::Polynomial( ); 83 | *polyfited2 = polyfit.getPolynomial( ); 84 | std::cout << "polyfited2 :" << endl 85 | << polyfited2->getPolyCoeff( ).transpose( ) << std::endl; 86 | 87 | std::cout << ":----------------------------------------:" << std::endl; 88 | eigen_utils::Vector realroot = poly.getRealRoot( 0.0 ); 89 | std::cout << " Roots :" << endl << realroot.transpose( ) << std::endl; 90 | 91 | std::cout << ":----------------------------------------:" << std::endl; 92 | // eigen_utils::Vector polyn = eigen_utils::SwapSequence(coeff); 93 | eigen_utils::Vector polyn( 4 ); 94 | polyn << -5, -1, 5, 1; 95 | std::cout << "polyn :" << endl << polyn.transpose( ) << std::endl; 96 | math_utils::Polynomial poly2( polyn ); 97 | eigen_utils::Vector realroot2 = poly2.getRealRoot( 0.0 ); 98 | std::cout << " Roots :" << endl << realroot2.transpose( ) << std::endl; 99 | 100 | std::cout << ":----------------------------------------:" << std::endl; 101 | eigen_utils::Vector poly3( 4 ); 102 | poly3 << -5, 1, 0, 0; 103 | std::cout << "polyn :" << endl << poly3.transpose( ) << std::endl; 104 | math_utils::Polynomial polyn3( poly3 ); 105 | double realroot3 = polyn3.getOneRealRoot( 0.0, -100, 100 ); 106 | std::cout << " One Roots :" << endl << realroot3 << std::endl; 107 | 108 | std::cout << ":----------------------------------------:" << std::endl; 109 | math_utils::Polynomial polyn4; 110 | polyn4 = polyn3; 111 | std::cout << "polyn3 :" << endl << polyn3.getPolyCoeff( ).transpose( ) << std::endl; 112 | std::cout << "polyn4 :" << endl << polyn4.getPolyCoeff( ).transpose( ) << std::endl; 113 | std::cout << "polyn4 :" << endl << polyn4.toString( ) << std::endl; 114 | } 115 | 116 | int 117 | main( ) 118 | { 119 | eigen_utils::Vector vec( 3 ); 120 | std::cout << "size :" << endl << vec.size( ) << std::endl; 121 | vec << 1, 2, 1; 122 | vec = eigen_utils::pushback( vec, 22 ); 123 | std::cout << "vec :" << endl << vec.transpose( ) << std::endl; 124 | 125 | test_poly( ); 126 | std::cout << " constant?" << std::endl; 127 | 128 | // double q_in[4] = {1,2,3,4}; 129 | // double q_out[4] ; 130 | 131 | sys_utils::print_color::PrintWarning( "warning" ); 132 | sys_utils::print_color::PrintError( "PrintError" ); 133 | sys_utils::print_color::PrintInfo( "PrintInfo" ); 134 | 135 | std::cout << ":----------------------------------------:" << std::endl; 136 | fit_test( ); 137 | 138 | return 0; 139 | } 140 | -------------------------------------------------------------------------------- /src/code_utils/src/sumpixel_test.cpp: -------------------------------------------------------------------------------- 1 | #define BACKWARD_HAS_DW 1 2 | #include "code_utils/backward.hpp" 3 | namespace backward 4 | { 5 | backward::SignalHandling sh; 6 | } 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | using namespace cv; 15 | 16 | double t; 17 | 18 | void 19 | showImg( std::string name, cv::Mat img ) 20 | { 21 | cv::namedWindow( name, WINDOW_NORMAL ); 22 | cv::imshow( name, img ); 23 | } 24 | 25 | void 26 | sumPixelRow( const cv::Mat& img, cv::Mat& integral ) 27 | { 28 | if ( img.type( ) == CV_8UC1 ) 29 | integral = cv::Mat( img.rows, img.cols, CV_32SC1 ); 30 | 31 | int nr = img.rows, nc = img.cols; 32 | int index_col, index_row; 33 | 34 | const uchar* pImg = img.ptr< uchar >( 0 ); 35 | int* pIntegral = integral.ptr< int >( 0 ); 36 | 37 | for ( index_row = 0; index_row < nr; ++index_row ) 38 | { 39 | pImg = img.ptr< uchar >( index_row ); 40 | pIntegral = integral.ptr< int >( index_row ); 41 | 42 | pIntegral[0] = pImg[0]; 43 | 44 | for ( index_col = 1; index_col < nc; ++index_col ) 45 | { 46 | pIntegral[index_col] = pIntegral[index_col - 1] + pImg[index_col]; 47 | } 48 | } 49 | } 50 | 51 | void 52 | sumPixelRow2( const cv::Mat& img, cv::Mat& integral ) 53 | { 54 | if ( img.type( ) == CV_8UC1 ) 55 | integral = cv::Mat( img.rows, img.cols, CV_32SC1 ); 56 | 57 | int nr = img.rows, nc = img.cols; 58 | int index_col, index_row; 59 | 60 | const uchar* pImg = img.ptr< uchar >( 0 ); 61 | int* pIntegral = integral.ptr< int >( 0 ); 62 | 63 | pIntegral[0] = pImg[0]; 64 | for ( index_col = 1; index_col < nc; ++index_col ) 65 | { 66 | pIntegral[index_col] = pIntegral[index_col - 1] + pImg[index_col]; 67 | } 68 | 69 | for ( index_row = 1; index_row < nr; ++index_row ) 70 | { 71 | pImg = img.ptr< uchar >( index_row ); 72 | pIntegral = integral.ptr< int >( index_row ); 73 | for ( index_col = 0; index_col < nc; ++index_col ) 74 | { 75 | pIntegral[index_col] = pIntegral[index_col - 1] + pImg[index_col]; 76 | } 77 | } 78 | } 79 | 80 | void 81 | test1( ) 82 | { 83 | Mat img; 84 | Mat img1 = imread( "/home/gao/IMG_1.png", CV_LOAD_IMAGE_GRAYSCALE ); 85 | 86 | sys_utils::tic::TicTocPart time; 87 | 88 | for ( int i = 0; i < 100; i++ ) 89 | sumPixelRow( img1, img ); 90 | 91 | std::cout << "sumPixelRow cost " << time.toc( ) << " ms\n"; 92 | 93 | cv::Mat img2; 94 | normalize( img, img2, 0, 255, CV_MINMAX ); 95 | Mat imageIntegralNorm; 96 | convertScaleAbs( img2, imageIntegralNorm ); 97 | 98 | showImg( "src1", img1 ); 99 | showImg( "dst1", img ); 100 | showImg( "dst11", imageIntegralNorm ); 101 | } 102 | 103 | void 104 | test2( ) 105 | { 106 | Mat img; 107 | Mat img1 = imread( "/home/gao/IMG_1.png", CV_LOAD_IMAGE_GRAYSCALE ); 108 | 109 | sys_utils::tic::TicTocPart time; 110 | 111 | for ( int i = 0; i < 100; i++ ) 112 | integral( img1, img, img1.type( ) ); 113 | 114 | std::cout << "sumPixelRow cost " << time.toc( ) << " ms\n"; 115 | 116 | cv::Mat img2; 117 | normalize( img, img2, 0, 255, CV_MINMAX ); 118 | Mat imageIntegralNorm; 119 | convertScaleAbs( img2, imageIntegralNorm ); 120 | 121 | showImg( "src2", img1 ); 122 | showImg( "dst2", img ); 123 | showImg( "dst22", imageIntegralNorm ); 124 | } 125 | 126 | int 127 | main( ) 128 | { 129 | test1( ); 130 | test2( ); 131 | 132 | waitKey( 0 ); 133 | 134 | return 0; 135 | } 136 | -------------------------------------------------------------------------------- /src/imu_utils/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | -------------------------------------------------------------------------------- /src/imu_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(imu_utils) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | set(CMAKE_BUILD_TYPE "Release") 6 | set(CMAKE_CXX_FLAGS "-std=c++11") 7 | #-DEIGEN_USE_MKL_ALL") 8 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -fPIC") 9 | 10 | ## Find catkin macros and libraries 11 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 12 | ## is used, also find other catkin packages 13 | find_package(catkin REQUIRED 14 | roscpp 15 | std_msgs 16 | geometry_msgs 17 | nav_msgs 18 | code_utils 19 | ) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | # find_package(Boost REQUIRED COMPONENTS system) 23 | find_package(OpenCV REQUIRED) 24 | find_package(Ceres REQUIRED) 25 | find_package(Eigen3 REQUIRED) 26 | include_directories( 27 | ${catkin_INCLUDE_DIRS} 28 | ${CERES_INCLUDE_DIRS} 29 | ${EIGEN3_INCLUDE_DIR} 30 | ) 31 | 32 | catkin_package() 33 | 34 | set(ACC_LIB_SOURCE_FILES 35 | ${PROJECT_SOURCE_DIR}/src/acc_lib/allan_acc.cpp 36 | ${PROJECT_SOURCE_DIR}/src/acc_lib/fitallan_acc.cpp 37 | ) 38 | set(GYR_LIB_SOURCE_FILES 39 | ${PROJECT_SOURCE_DIR}/src/gyr_lib/allan_gyr.cpp 40 | ${PROJECT_SOURCE_DIR}/src/gyr_lib/fitallan_gyr.cpp 41 | ) 42 | 43 | add_executable(imu_an 44 | src/imu_an.cpp 45 | ${GYR_LIB_SOURCE_FILES} 46 | ${ACC_LIB_SOURCE_FILES} 47 | ) 48 | 49 | target_link_libraries(imu_an ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 50 | -------------------------------------------------------------------------------- /src/imu_utils/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 gaowenliang 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/imu_utils/README.md: -------------------------------------------------------------------------------- 1 | # imu_utils 2 | 3 | A ROS package tool to analyze the IMU performance. C++ version of Allan Variance Tool. 4 | The figures are drawn by Matlab, in `scripts`. 5 | 6 | Actually, just analyze the Allan Variance for the IMU data. Collect the data while the IMU is Stationary, with a two hours duration. 7 | 8 | ## refrence 9 | 10 | Refrence technical report: [`Allan Variance: Noise Analysis for Gyroscopes`](http://cache.freescale.com/files/sensors/doc/app_note/AN5087.pdf "Allan Variance: Noise Analysis for Gyroscopes"), [`vectornav gyroscope`](https://www.vectornav.com/support/library/gyroscope "vectornav gyroscope") and 11 | [`An introduction to inertial navigation`](http://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.html "An introduction to inertial navigation"). 12 | 13 | ``` 14 | Woodman, O.J., 2007. An introduction to inertial navigation (No. UCAM-CL-TR-696). University of Cambridge, Computer Laboratory. 15 | ``` 16 | Refrence Matlab code: [`GyroAllan`](https://github.com/XinLiGitHub/GyroAllan "GyroAllan") 17 | 18 | ## IMU Noise Values 19 | 20 | Parameter | YAML element | Symbol | Units 21 | --- | --- | --- | --- 22 | Gyroscope "white noise" | `gyr_n` | | 23 | Accelerometer "white noise" | `acc_n` | | 24 | Gyroscope "bias Instability" | `gyr_w` | | 25 | Accelerometer "bias Instability" | `acc_w` | | 26 | 27 | * White noise is at tau=1; 28 | 29 | * Bias Instability is around the minimum; 30 | 31 | (according to technical report: [`Allan Variance: Noise Analysis for Gyroscopes`](http://cache.freescale.com/files/sensors/doc/app_note/AN5087.pdf "Allan Variance: Noise Analysis for Gyroscopes")) 32 | 33 | ## sample test 34 | 35 | 36 | 37 | 38 | * blue : Vi-Sensor, ADIS16448, `200Hz` 39 | * red : 3dm-Gx4, `500Hz` 40 | * green : DJI-A3, `400Hz` 41 | * black : DJI-N3, `400Hz` 42 | * circle : xsens-MTI-100, `100Hz` 43 | 44 | ## How to build and run? 45 | 46 | ### to build 47 | 48 | ``` 49 | sudo apt-get install libdw-dev 50 | ``` 51 | 52 | * download required [`code_utils`](https://github.com/gaowenliang/code_utils "code_utils"); 53 | 54 | * put the ROS package `imu_utils` and `code_utils` into your workspace, usually named `catkin_ws`; 55 | 56 | * cd to your workspace, build with `catkin_make`; 57 | 58 | 59 | ### to run 60 | 61 | * collect the data while the IMU is Stationary, with a two hours duration; 62 | 63 | * (or) play rosbag dataset; 64 | 65 | ``` 66 | rosbag play -r 200 imu_A3.bag 67 | ``` 68 | 69 | * roslaunch the rosnode; 70 | 71 | ``` 72 | roslaunch imu_utils A3.launch 73 | ``` 74 | 75 | Be careful of your roslaunch file: 76 | 77 | ``` 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | ``` 88 | 89 | ### sample output: 90 | 91 | ``` 92 | type: IMU 93 | name: A3 94 | Gyr: 95 | unit: " rad/s" 96 | avg-axis: 97 | gyr_n: 1.0351286977809465e-04 98 | gyr_w: 2.9438676109223402e-05 99 | x-axis: 100 | gyr_n: 1.0312669892959053e-04 101 | gyr_w: 3.3765827874234673e-05 102 | y-axis: 103 | gyr_n: 1.0787155789128671e-04 104 | gyr_w: 3.1970693666470835e-05 105 | z-axis: 106 | gyr_n: 9.9540352513406743e-05 107 | gyr_w: 2.2579506786964707e-05 108 | Acc: 109 | unit: " m/s^2" 110 | avg-axis: 111 | acc_n: 1.3985049290745563e-03 112 | acc_w: 6.3249251509920116e-04 113 | x-axis: 114 | acc_n: 1.1687799474421937e-03 115 | acc_w: 5.3044554054317266e-04 116 | y-axis: 117 | acc_n: 1.2050535351630543e-03 118 | acc_w: 6.0281218607825414e-04 119 | z-axis: 120 | acc_n: 1.8216813046184213e-03 121 | acc_w: 7.6421981867617645e-04 122 | ``` 123 | 124 | ## dataset 125 | 126 | DJI A3: `400Hz` 127 | 128 | Download link: [`百度网盘`](https://pan.baidu.com/s/1jJYg8R0 "DJI A3") 129 | 130 | 131 | DJI A3: `400Hz` 132 | 133 | Download link: [`百度网盘`](https://pan.baidu.com/s/1pLXGqx1 "DJI N3") 134 | 135 | 136 | ADIS16448: `200Hz` 137 | 138 | Download link:[`百度网盘`](https://pan.baidu.com/s/1dGd0mn3 "ADIS16448") 139 | 140 | 3dM-GX4: `500Hz` 141 | 142 | Download link:[`百度网盘`](https://pan.baidu.com/s/1ggcan9D "GX4") 143 | 144 | xsens-MTI-100: `100Hz` 145 | 146 | Download link:[`百度网盘`](https://pan.baidu.com/s/1i64xkgP "MTI-100") 147 | -------------------------------------------------------------------------------- /src/imu_utils/_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-leap-day -------------------------------------------------------------------------------- /src/imu_utils/data/A3_imu_param.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | type: IMU 4 | name: A3 5 | Gyr: 6 | unit: " rad/s" 7 | avg-axis: 8 | gyr_n: 2.1212796973402379e-01 9 | gyr_w: 6.8714855284767210e-04 10 | x-axis: 11 | gyr_n: 2.1161794469601317e-01 12 | gyr_w: 4.7057814164227406e-04 13 | y-axis: 14 | gyr_n: 2.0830451415422069e-01 15 | gyr_w: 8.5083648541348566e-04 16 | z-axis: 17 | gyr_n: 2.1646145035183750e-01 18 | gyr_w: 7.4003103148725696e-04 19 | Acc: 20 | unit: " m/s^2" 21 | avg-axis: 22 | acc_n: 2.6782190207314710e-01 23 | acc_w: 3.2943715227012351e-03 24 | x-axis: 25 | acc_n: 2.7160999239114814e-01 26 | acc_w: 3.2443492128074914e-03 27 | y-axis: 28 | acc_n: 2.6754676366468833e-01 29 | acc_w: 3.5364015693452717e-03 30 | z-axis: 31 | acc_n: 2.6430895016360473e-01 32 | acc_w: 3.1023637859509414e-03 33 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_acc_t.txt: -------------------------------------------------------------------------------- 1 | 0.005004040523 2 | 0.01000808105 3 | 0.01501212157 4 | 0.02001616209 5 | 0.02502020262 6 | 0.03002424314 7 | 0.03502828366 8 | 0.04003232419 9 | 0.04503636471 10 | 0.05004040523 11 | 0.05504444576 12 | 0.06505252681 13 | 0.07506060785 14 | 0.0850686889 15 | 0.09507676995 16 | 0.1100888915 17 | 0.1251010131 18 | 0.1401131347 19 | 0.1601292968 20 | 0.1851494994 21 | 0.210169702 22 | 0.2401939451 23 | 0.2752222288 24 | 0.3102505125 25 | 0.3552868772 26 | 0.4053272824 27 | 0.4653757687 28 | 0.5304282955 29 | 0.6054889033 30 | 0.6905575922 31 | 0.7856343622 32 | 0.9007272942 33 | 1.025828307 34 | 1.170945482 35 | 1.33607882 36 | 1.52623236 37 | 1.746410143 38 | 1.991608128 39 | 2.276838438 40 | 2.602101072 41 | 2.972400071 42 | 3.392739475 43 | 3.873127365 44 | 4.423571823 45 | 5.054080929 46 | 5.774662764 47 | 6.59532541 48 | 7.536085028 49 | 8.6069497 50 | 9.827935588 51 | 11.22906693 52 | 12.82535586 53 | 14.64682661 54 | 16.73351151 55 | 19.1154348 56 | 21.8326288 57 | 24.94013797 58 | 28.4880027 59 | 32.53627148 60 | 37.17001301 61 | 42.4542798 62 | 48.49415671 63 | 55.3947286 64 | 63.27609242 65 | 72.27836132 66 | 82.56666864 67 | 94.31115175 68 | 107.7319884 69 | 123.0593646 70 | 140.5685023 71 | 160.5646483 72 | 183.4130973 73 | 209.5091686 74 | 239.318238 75 | 273.3657298 76 | 312.2621367 77 | 356.6930126 78 | 407.4389875 79 | 465.410797 80 | 531.6342693 81 | 607.2753458 82 | 693.6801135 83 | 792.3748048 84 | 905.1158378 85 | 1033.899825 86 | 1181.003604 87 | 1349.039285 88 | 1540.979267 89 | 1760.236307 90 | 2010.683531 91 | 2296.769532 92 | 2623.558398 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_acc_x.txt: -------------------------------------------------------------------------------- 1 | 0.2689731882 2 | 0.1901474437 3 | 0.1551833686 4 | 0.1343102581 5 | 0.1201145579 6 | 0.1096826646 7 | 0.1015964518 8 | 0.09506657144 9 | 0.08965356285 10 | 0.0850580481 11 | 0.08109469348 12 | 0.07456306575 13 | 0.06938866742 14 | 0.06517509167 15 | 0.06164663773 16 | 0.05731745502 17 | 0.05375614665 18 | 0.05076178978 19 | 0.04745078445 20 | 0.04409667582 21 | 0.04135722338 22 | 0.03865088317 23 | 0.03611120354 24 | 0.0340173956 25 | 0.03177469191 26 | 0.02975813108 27 | 0.02780069203 28 | 0.02605802181 29 | 0.02440112454 30 | 0.02286687825 31 | 0.02145255441 32 | 0.02002201749 33 | 0.01875826314 34 | 0.01756305302 35 | 0.01646283032 36 | 0.01540325406 37 | 0.01442674476 38 | 0.01350036939 39 | 0.01258955156 40 | 0.01172501895 41 | 0.01093110301 42 | 0.01021174597 43 | 0.009555240827 44 | 0.008958597032 45 | 0.008410434325 46 | 0.007916417989 47 | 0.007485705522 48 | 0.007037414452 49 | 0.006614237075 50 | 0.006195330367 51 | 0.005829601595 52 | 0.00545302783 53 | 0.005115658108 54 | 0.004832750788 55 | 0.004592086115 56 | 0.004360350235 57 | 0.004120318759 58 | 0.003913838785 59 | 0.003714196918 60 | 0.003540393908 61 | 0.003400696392 62 | 0.003314983126 63 | 0.003274662633 64 | 0.003286255948 65 | 0.003325105669 66 | 0.003392759341 67 | 0.00349025443 68 | 0.003591729365 69 | 0.003685079664 70 | 0.003788934404 71 | 0.00390426181 72 | 0.004068089654 73 | 0.004281896908 74 | 0.004523151457 75 | 0.004753644456 76 | 0.004958593753 77 | 0.005121371591 78 | 0.005220123301 79 | 0.005289780643 80 | 0.005415714106 81 | 0.005651610001 82 | 0.005975472957 83 | 0.00634775196 84 | 0.006726545775 85 | 0.007075311783 86 | 0.007457051924 87 | 0.007845541446 88 | 0.008225581472 89 | 0.008699986929 90 | 0.008973201272 91 | 0.008562649189 92 | 0.008193683753 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_acc_y.txt: -------------------------------------------------------------------------------- 1 | 0.2685902629 2 | 0.1898135368 3 | 0.1550672258 4 | 0.1342693099 5 | 0.1200314649 6 | 0.1096248823 7 | 0.1015275209 8 | 0.09498454984 9 | 0.08956485202 10 | 0.08498301097 11 | 0.08103048309 12 | 0.07455620626 13 | 0.06946650393 14 | 0.06530193909 15 | 0.06180363468 16 | 0.05744429368 17 | 0.05385981772 18 | 0.05085886927 19 | 0.04754441708 20 | 0.04419797863 21 | 0.0414639994 22 | 0.03874300393 23 | 0.03614396573 24 | 0.0340050598 25 | 0.03175651998 26 | 0.02970231855 27 | 0.02768849046 28 | 0.02595137775 29 | 0.0243018217 30 | 0.02274076858 31 | 0.02130371923 32 | 0.01988447982 33 | 0.01860499138 34 | 0.01738966428 35 | 0.01627682459 36 | 0.01524221085 37 | 0.01427841011 38 | 0.01342246565 39 | 0.01260896183 40 | 0.01186468251 41 | 0.01115738731 42 | 0.01047734651 43 | 0.009822753759 44 | 0.009191729875 45 | 0.00859938375 46 | 0.00801621746 47 | 0.007477600987 48 | 0.006972041232 49 | 0.006503913112 50 | 0.006093445571 51 | 0.005722546228 52 | 0.005410050049 53 | 0.005132039789 54 | 0.004891792257 55 | 0.004644583916 56 | 0.004384291109 57 | 0.004124146104 58 | 0.003884452673 59 | 0.003685509198 60 | 0.003528846964 61 | 0.003423561229 62 | 0.003382165802 63 | 0.003408305951 64 | 0.003466199182 65 | 0.003547038079 66 | 0.003636562379 67 | 0.003743573409 68 | 0.003870501276 69 | 0.00404049186 70 | 0.004270306828 71 | 0.004556786889 72 | 0.004898065287 73 | 0.005277001142 74 | 0.005705452629 75 | 0.006165002757 76 | 0.006659953298 77 | 0.007155116145 78 | 0.007626758543 79 | 0.008053922347 80 | 0.008416939018 81 | 0.008698912284 82 | 0.008934262632 83 | 0.009171167637 84 | 0.009383171027 85 | 0.009508062502 86 | 0.009503969939 87 | 0.009592587673 88 | 0.01010419969 89 | 0.01094350271 90 | 0.01230527442 91 | 0.01413847472 92 | 0.01575357972 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_acc_z.txt: -------------------------------------------------------------------------------- 1 | 0.2691040037 2 | 0.1901711897 3 | 0.1552147674 4 | 0.134331049 5 | 0.120154837 6 | 0.1097271657 7 | 0.1016436177 8 | 0.09510617917 9 | 0.08966453808 10 | 0.08506412747 11 | 0.08110765662 12 | 0.07458415798 13 | 0.06940968357 14 | 0.06518299597 15 | 0.06165422792 16 | 0.05726221912 17 | 0.05370073979 18 | 0.05073187713 19 | 0.04747872884 20 | 0.04414379438 21 | 0.04140171054 22 | 0.03871454587 23 | 0.03614099203 24 | 0.03399825285 25 | 0.03174954976 26 | 0.02975647139 27 | 0.0278191951 28 | 0.02610947384 29 | 0.02449590249 30 | 0.02298435847 31 | 0.02155341301 32 | 0.02011453479 33 | 0.01881732201 34 | 0.0175977422 35 | 0.01645122434 36 | 0.0153805715 37 | 0.0143440706 38 | 0.01339270127 39 | 0.01247797879 40 | 0.01162201655 41 | 0.01084628696 42 | 0.0101637677 43 | 0.009541333564 44 | 0.008941781382 45 | 0.008384887397 46 | 0.00785026247 47 | 0.007338074838 48 | 0.006866754657 49 | 0.006424859338 50 | 0.006011690531 51 | 0.005630848503 52 | 0.005311664723 53 | 0.005026297605 54 | 0.004783379949 55 | 0.004551135339 56 | 0.004318703507 57 | 0.004072595445 58 | 0.003817318701 59 | 0.003600991295 60 | 0.003407624861 61 | 0.00326806254 62 | 0.003171404547 63 | 0.003137832631 64 | 0.003122875998 65 | 0.003139671454 66 | 0.003190651386 67 | 0.003293651039 68 | 0.003432124717 69 | 0.003577386416 70 | 0.003713816993 71 | 0.003836906702 72 | 0.0039535374 73 | 0.004064805744 74 | 0.004204134756 75 | 0.004398992281 76 | 0.004650101345 77 | 0.004954540559 78 | 0.00529888487 79 | 0.005666164615 80 | 0.006057535531 81 | 0.006508917719 82 | 0.007027714383 83 | 0.007640611538 84 | 0.008316538012 85 | 0.008930204419 86 | 0.009325363401 87 | 0.009424545421 88 | 0.009101403002 89 | 0.00841200864 90 | 0.007598535949 91 | 0.00694686626 92 | 0.006197394378 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_gyr_t.txt: -------------------------------------------------------------------------------- 1 | 0.005004040523 2 | 0.01000808105 3 | 0.01501212157 4 | 0.02001616209 5 | 0.02502020262 6 | 0.03002424314 7 | 0.03502828366 8 | 0.04003232419 9 | 0.04503636471 10 | 0.05004040523 11 | 0.05504444576 12 | 0.06505252681 13 | 0.07506060785 14 | 0.0850686889 15 | 0.09507676995 16 | 0.1100888915 17 | 0.1251010131 18 | 0.1401131347 19 | 0.1601292968 20 | 0.1851494994 21 | 0.210169702 22 | 0.2401939451 23 | 0.2752222288 24 | 0.3102505125 25 | 0.3552868772 26 | 0.4053272824 27 | 0.4653757687 28 | 0.5304282955 29 | 0.6054889033 30 | 0.6905575922 31 | 0.7856343622 32 | 0.9007272942 33 | 1.025828307 34 | 1.170945482 35 | 1.33607882 36 | 1.52623236 37 | 1.746410143 38 | 1.991608128 39 | 2.276838438 40 | 2.602101072 41 | 2.972400071 42 | 3.392739475 43 | 3.873127365 44 | 4.423571823 45 | 5.054080929 46 | 5.774662764 47 | 6.59532541 48 | 7.536085028 49 | 8.6069497 50 | 9.827935588 51 | 11.22906693 52 | 12.82535586 53 | 14.64682661 54 | 16.73351151 55 | 19.1154348 56 | 21.8326288 57 | 24.94013797 58 | 28.4880027 59 | 32.53627148 60 | 37.17001301 61 | 42.4542798 62 | 48.49415671 63 | 55.3947286 64 | 63.27609242 65 | 72.27836132 66 | 82.56666864 67 | 94.31115175 68 | 107.7319884 69 | 123.0593646 70 | 140.5685023 71 | 160.5646483 72 | 183.4130973 73 | 209.5091686 74 | 239.318238 75 | 273.3657298 76 | 312.2621367 77 | 356.6930126 78 | 407.4389875 79 | 465.410797 80 | 531.6342693 81 | 607.2753458 82 | 693.6801135 83 | 792.3748048 84 | 905.1158378 85 | 1033.899825 86 | 1181.003604 87 | 1349.039285 88 | 1540.979267 89 | 1760.236307 90 | 2010.683531 91 | 2296.769532 92 | 2623.558398 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_gyr_x.txt: -------------------------------------------------------------------------------- 1 | 43708.30598 2 | 30920.36716 3 | 25239.74935 4 | 21874.75003 5 | 19567.3213 6 | 17858.19801 7 | 16525.94515 8 | 15453.09706 9 | 14563.91734 10 | 13816.76435 11 | 13175.65317 12 | 12121.54768 13 | 11284.06665 14 | 10602.94538 15 | 10031.93452 16 | 9328.28871 17 | 8757.925029 18 | 8279.304216 19 | 7743.640823 20 | 7196.706511 21 | 6750.302352 22 | 6310.660152 23 | 5898.094463 24 | 5558.253337 25 | 5201.784431 26 | 4880.049863 27 | 4555.518884 28 | 4261.862941 29 | 3984.532829 30 | 3727.349408 31 | 3488.858573 32 | 3253.123038 33 | 3043.415568 34 | 2844.14171 35 | 2658.236904 36 | 2483.455534 37 | 2321.656231 38 | 2175.714253 39 | 2041.072265 40 | 1915.743386 41 | 1797.48071 42 | 1688.690604 43 | 1585.704188 44 | 1488.24275 45 | 1392.732674 46 | 1303.533435 47 | 1217.768222 48 | 1137.009372 49 | 1063.465675 50 | 996.0514798 51 | 927.7234798 52 | 859.521775 53 | 795.8642096 54 | 738.1945315 55 | 688.2656039 56 | 644.9226572 57 | 611.9018254 58 | 582.1779837 59 | 556.5922133 60 | 529.9178457 61 | 502.1246229 62 | 475.848677 63 | 446.8434383 64 | 414.4215193 65 | 382.7363136 66 | 356.9445227 67 | 333.762583 68 | 313.2205853 69 | 296.6784609 70 | 277.7896461 71 | 259.1235875 72 | 240.0798022 73 | 221.3476024 74 | 205.4090766 75 | 195.4635399 76 | 185.3819676 77 | 174.3596034 78 | 160.6018348 79 | 140.7478472 80 | 124.6542193 81 | 109.7207687 82 | 99.20602015 83 | 89.0481616 84 | 88.08183745 85 | 87.09316431 86 | 89.0140111 87 | 90.24567698 88 | 97.28750678 89 | 103.5158403 90 | 107.0222277 91 | 113.6057679 92 | 120.0628173 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_gyr_y.txt: -------------------------------------------------------------------------------- 1 | 43682.18939 2 | 30940.09964 3 | 25266.03464 4 | 21881.98649 5 | 19562.61611 6 | 17847.84479 7 | 16519.0061 8 | 15444.04668 9 | 14556.76357 10 | 13805.11422 11 | 13160.0588 12 | 12102.3226 13 | 11262.6586 14 | 10580.71594 15 | 10009.66481 16 | 9304.010611 17 | 8731.477298 18 | 8251.333437 19 | 7721.77233 20 | 7178.812002 21 | 6736.934734 22 | 6301.530909 23 | 5889.311168 24 | 5549.740302 25 | 5186.193821 26 | 4853.460852 27 | 4520.871932 28 | 4228.615148 29 | 3949.064192 30 | 3690.193507 31 | 3453.563711 32 | 3221.387212 33 | 3011.119169 34 | 2812.098124 35 | 2627.314411 36 | 2457.010376 37 | 2300.644495 38 | 2161.928806 39 | 2025.041134 40 | 1892.406283 41 | 1765.445742 42 | 1648.56048 43 | 1540.578835 44 | 1446.257708 45 | 1356.511353 46 | 1274.423125 47 | 1196.864669 48 | 1119.788741 49 | 1042.421961 50 | 973.8165573 51 | 913.0920219 52 | 856.2799944 53 | 805.4097036 54 | 756.5995 55 | 707.659571 56 | 659.7395543 57 | 614.4828143 58 | 572.4093795 59 | 532.4173826 60 | 496.3948019 61 | 457.5855114 62 | 420.6024902 63 | 384.8880928 64 | 353.4172096 65 | 322.2169923 66 | 302.2091701 67 | 287.599304 68 | 276.9800786 69 | 265.507932 70 | 258.3469281 71 | 249.193265 72 | 237.3192976 73 | 229.3880826 74 | 224.493838 75 | 215.2590637 76 | 207.0263663 77 | 201.0998692 78 | 193.8203254 79 | 188.1659057 80 | 181.6511956 81 | 176.9227968 82 | 174.6546565 83 | 173.9031837 84 | 174.8012631 85 | 177.5148567 86 | 188.409091 87 | 206.7978598 88 | 230.3599522 89 | 251.3692283 90 | 276.9190071 91 | 289.9441449 92 | 291.4633427 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_gyr_z.txt: -------------------------------------------------------------------------------- 1 | 43741.60162 2 | 30937.26868 3 | 25260.39705 4 | 21885.55836 5 | 19570.96103 6 | 17857.38585 7 | 16524.97425 8 | 15448.50546 9 | 14558.39991 10 | 13805.74963 11 | 13158.36309 12 | 12099.74081 13 | 11256.16072 14 | 10568.40672 15 | 9994.38423 16 | 9292.861853 17 | 8721.572191 18 | 8245.577734 19 | 7723.213686 20 | 7192.042767 21 | 6755.780908 22 | 6325.912329 23 | 5915.532893 24 | 5578.370643 25 | 5219.645495 26 | 4894.187372 27 | 4570.344691 28 | 4280.020643 29 | 3998.6763 30 | 3730.529136 31 | 3481.453694 32 | 3243.859708 33 | 3040.601133 34 | 2848.113221 35 | 2672.088824 36 | 2508.846109 37 | 2344.186806 38 | 2185.519077 39 | 2037.057922 40 | 1905.275605 41 | 1782.333359 42 | 1672.721708 43 | 1575.95241 44 | 1487.570598 45 | 1403.848179 46 | 1321.695758 47 | 1246.982406 48 | 1176.750299 49 | 1106.852829 50 | 1037.944708 51 | 972.6144991 52 | 909.1645787 53 | 849.189779 54 | 796.1019512 55 | 748.1790689 56 | 702.3770954 57 | 659.63032 58 | 622.2188119 59 | 584.0853965 60 | 544.9332009 61 | 506.8226055 62 | 471.7076202 63 | 439.6745256 64 | 406.77661 65 | 382.3595118 66 | 359.5564373 67 | 339.3696435 68 | 321.3477508 69 | 307.8798774 70 | 296.2013224 71 | 284.9065192 72 | 271.6074734 73 | 255.6077114 74 | 235.2573971 75 | 215.4821601 76 | 197.0297009 77 | 184.8985356 78 | 176.4311343 79 | 169.5710413 80 | 161.5449825 81 | 152.0664742 82 | 145.3978406 83 | 143.7990767 84 | 145.8456743 85 | 149.3148759 86 | 153.4938939 87 | 164.8113995 88 | 180.3207476 89 | 204.1025927 90 | 226.9675954 91 | 248.5575908 92 | 277.4774172 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_sim_acc_t.txt: -------------------------------------------------------------------------------- 1 | 0.005004040523 2 | 0.01000808105 3 | 0.01501212157 4 | 0.02001616209 5 | 0.02502020262 6 | 0.03002424314 7 | 0.03502828366 8 | 0.04003232419 9 | 0.04503636471 10 | 0.05004040523 11 | 0.05504444576 12 | 0.06505252681 13 | 0.07506060785 14 | 0.0850686889 15 | 0.09507676995 16 | 0.1100888915 17 | 0.1251010131 18 | 0.1401131347 19 | 0.1601292968 20 | 0.1851494994 21 | 0.210169702 22 | 0.2401939451 23 | 0.2752222288 24 | 0.3102505125 25 | 0.3552868772 26 | 0.4053272824 27 | 0.4653757687 28 | 0.5304282955 29 | 0.6054889033 30 | 0.6905575922 31 | 0.7856343622 32 | 0.9007272942 33 | 1.025828307 34 | 1.170945482 35 | 1.33607882 36 | 1.52623236 37 | 1.746410143 38 | 1.991608128 39 | 2.276838438 40 | 2.602101072 41 | 2.972400071 42 | 3.392739475 43 | 3.873127365 44 | 4.423571823 45 | 5.054080929 46 | 5.774662764 47 | 6.59532541 48 | 7.536085028 49 | 8.6069497 50 | 9.827935588 51 | 11.22906693 52 | 12.82535586 53 | 14.64682661 54 | 16.73351151 55 | 19.1154348 56 | 21.8326288 57 | 24.94013797 58 | 28.4880027 59 | 32.53627148 60 | 37.17001301 61 | 42.4542798 62 | 48.49415671 63 | 55.3947286 64 | 63.27609242 65 | 72.27836132 66 | 82.56666864 67 | 94.31115175 68 | 107.7319884 69 | 123.0593646 70 | 140.5685023 71 | 160.5646483 72 | 183.4130973 73 | 209.5091686 74 | 239.318238 75 | 273.3657298 76 | 312.2621367 77 | 356.6930126 78 | 407.4389875 79 | 465.410797 80 | 531.6342693 81 | 607.2753458 82 | 693.6801135 83 | 792.3748048 84 | 905.1158378 85 | 1033.899825 86 | 1181.003604 87 | 1349.039285 88 | 1540.979267 89 | 1760.236307 90 | 2010.683531 91 | 2296.769532 92 | 2623.558398 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_sim_acc_x.txt: -------------------------------------------------------------------------------- 1 | 0.3308331551 2 | 0.2138706817 3 | 0.1688113496 4 | 0.1436116495 5 | 0.1270437753 6 | 0.1151107955 7 | 0.1059970464 8 | 0.09874600416 9 | 0.09280048663 10 | 0.08781138349 11 | 0.08354748982 12 | 0.07660089132 13 | 0.07113936478 14 | 0.06669992297 15 | 0.0629992136 16 | 0.058446646 17 | 0.05475664438 18 | 0.0516873461 19 | 0.0482976679 20 | 0.04487077486 21 | 0.0420831033 22 | 0.03933740736 23 | 0.0367260186 24 | 0.03457411008 25 | 0.03229329565 26 | 0.03022227087 27 | 0.02819512375 28 | 0.02640211908 29 | 0.0247055081 30 | 0.02312920221 31 | 0.02168114441 32 | 0.020246056 33 | 0.01896979977 34 | 0.01775454831 35 | 0.01662094762 36 | 0.01555149699 37 | 0.01453914896 38 | 0.01361630316 39 | 0.01273703897 40 | 0.01191717569 41 | 0.01115359678 42 | 0.01044395062 43 | 0.009779772476 44 | 0.009156995726 45 | 0.008573810539 46 | 0.008029364879 47 | 0.007523026736 48 | 0.007049461147 49 | 0.006610116825 50 | 0.006202197463 51 | 0.005821824642 52 | 0.00547058893 53 | 0.005146628735 54 | 0.004847910503 55 | 0.0045749704 56 | 0.004327419449 57 | 0.004104400679 58 | 0.003906314039 59 | 0.003733379974 60 | 0.003585460811 61 | 0.00346359853 62 | 0.003368011806 63 | 0.003299318829 64 | 0.003258000029 65 | 0.003244349213 66 | 0.003258483341 67 | 0.003300268466 68 | 0.003369429565 69 | 0.003465474647 70 | 0.003587883899 71 | 0.003736077495 72 | 0.003909661138 73 | 0.004108213701 74 | 0.004331577094 75 | 0.004579748057 76 | 0.004853006649 77 | 0.005151770777 78 | 0.005476660216 79 | 0.005828622407 80 | 0.006208724109 81 | 0.006618202892 82 | 0.00705856715 83 | 0.007531462083 84 | 0.00803878411 85 | 0.008582578254 86 | 0.009165070518 87 | 0.009788729384 88 | 0.0104561878 89 | 0.01117034909 90 | 0.01193425989 91 | 0.0127512726 92 | 0.01362493623 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_sim_acc_y.txt: -------------------------------------------------------------------------------- 1 | 0.3315360182 2 | 0.212885565 3 | 0.1675463457 4 | 0.1423049047 5 | 0.1257578589 6 | 0.1138640773 7 | 0.1047938777 8 | 0.09758579507 9 | 0.09168089765 10 | 0.08672955352 11 | 0.0825005281 12 | 0.07561571524 13 | 0.07020694308 14 | 0.06581297549 15 | 0.06215187682 16 | 0.05765006835 17 | 0.05400277421 18 | 0.05097001305 19 | 0.04762170024 20 | 0.04423764795 21 | 0.04148553662 22 | 0.03877544807 23 | 0.03619842541 24 | 0.03407517723 25 | 0.03182505138 26 | 0.02978214836 27 | 0.02778274256 28 | 0.02601443627 29 | 0.02434132171 30 | 0.0227869449 31 | 0.02135910761 32 | 0.01994412349 33 | 0.01868579714 34 | 0.01748766209 35 | 0.0163700687 36 | 0.01531576233 37 | 0.01431779997 38 | 0.01340812753 39 | 0.01254149212 40 | 0.01173350627 41 | 0.01098112228 42 | 0.01028205239 43 | 0.009627996246 44 | 0.009015000414 45 | 0.008441348038 46 | 0.007906278732 47 | 0.00740926145 48 | 0.006945172186 49 | 0.006515565499 50 | 0.006117860401 51 | 0.005748474653 52 | 0.005409185317 53 | 0.005098449136 54 | 0.004814629787 55 | 0.004558593273 56 | 0.004330345808 57 | 0.004129506477 58 | 0.003956862221 59 | 0.003813000485 60 | 0.00369819318 61 | 0.003613563279 62 | 0.003559466907 63 | 0.003536401569 64 | 0.003544573468 65 | 0.003583903813 66 | 0.003654106693 67 | 0.003754563213 68 | 0.003884714855 69 | 0.004043807549 70 | 0.004231250355 71 | 0.004446496356 72 | 0.004689356846 73 | 0.004959647302 74 | 0.005257559063 75 | 0.005583474737 76 | 0.005938118095 77 | 0.006322350941 78 | 0.006737252813 79 | 0.007184278121 80 | 0.007664990574 81 | 0.008181132971 82 | 0.008734756297 83 | 0.009328054146 84 | 0.00996351016 85 | 0.0106437725 86 | 0.01137169761 87 | 0.01215042966 88 | 0.01298330525 89 | 0.01387398755 90 | 0.01482630947 91 | 0.0158444775 92 | 0.01693293689 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_sim_acc_z.txt: -------------------------------------------------------------------------------- 1 | 0.3266676576 2 | 0.2099594908 3 | 0.1653120152 4 | 0.1404398427 5 | 0.1241281663 6 | 0.1124001922 7 | 0.1034545364 8 | 0.09634426718 9 | 0.09051874475 10 | 0.08563344446 11 | 0.08146046546 12 | 0.07466620165 13 | 0.06932798501 14 | 0.06499097633 15 | 0.06137710181 16 | 0.05693308397 17 | 0.05333239861 18 | 0.05033825723 19 | 0.04703244838 20 | 0.04369122117 21 | 0.04097384767 22 | 0.03829789487 23 | 0.03575327253 24 | 0.03365668074 25 | 0.03143477053 26 | 0.02941745851 27 | 0.027443078 28 | 0.02569688939 29 | 0.02404468983 30 | 0.02250973296 31 | 0.02109972387 32 | 0.0197023936 33 | 0.01845974929 34 | 0.01727652262 35 | 0.01617280313 36 | 0.01513154044 37 | 0.01414586311 38 | 0.01324730938 39 | 0.01239116043 40 | 0.01159281241 41 | 0.01084922487 42 | 0.01015810276 43 | 0.009511196986 44 | 0.008904540257 45 | 0.008336362403 46 | 0.007805825788 47 | 0.007312307052 48 | 0.006850597125 49 | 0.006422098061 50 | 0.006024071878 51 | 0.005652719437 52 | 0.005309576998 53 | 0.004992809124 54 | 0.004700406071 55 | 0.004432869016 56 | 0.004189793733 57 | 0.003970313583 58 | 0.003774796525 59 | 0.003603438256 60 | 0.003456081178 61 | 0.003333748059 62 | 0.00323665188 63 | 0.003165417174 64 | 0.003120545786 65 | 0.003102363786 66 | 0.003111024435 67 | 0.00314644027 68 | 0.003208364043 69 | 0.00329633112 70 | 0.003409828999 71 | 0.003548274711 72 | 0.003711249179 73 | 0.003898304089 74 | 0.004109238038 75 | 0.004343998533 76 | 0.004602805539 77 | 0.004886014987 78 | 0.005194179632 79 | 0.005528168421 80 | 0.005888968523 81 | 0.006277733587 82 | 0.00669587815 83 | 0.007144949859 84 | 0.007626737602 85 | 0.008143173597 86 | 0.008696363753 87 | 0.009288645724 88 | 0.009922514548 89 | 0.01060072297 90 | 0.01132616063 91 | 0.01210200808 92 | 0.0129316331 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_sim_gyr_t.txt: -------------------------------------------------------------------------------- 1 | 0.005004040523 2 | 0.01000808105 3 | 0.01501212157 4 | 0.02001616209 5 | 0.02502020262 6 | 0.03002424314 7 | 0.03502828366 8 | 0.04003232419 9 | 0.04503636471 10 | 0.05004040523 11 | 0.05504444576 12 | 0.06505252681 13 | 0.07506060785 14 | 0.0850686889 15 | 0.09507676995 16 | 0.1100888915 17 | 0.1251010131 18 | 0.1401131347 19 | 0.1601292968 20 | 0.1851494994 21 | 0.210169702 22 | 0.2401939451 23 | 0.2752222288 24 | 0.3102505125 25 | 0.3552868772 26 | 0.4053272824 27 | 0.4653757687 28 | 0.5304282955 29 | 0.6054889033 30 | 0.6905575922 31 | 0.7856343622 32 | 0.9007272942 33 | 1.025828307 34 | 1.170945482 35 | 1.33607882 36 | 1.52623236 37 | 1.746410143 38 | 1.991608128 39 | 2.276838438 40 | 2.602101072 41 | 2.972400071 42 | 3.392739475 43 | 3.873127365 44 | 4.423571823 45 | 5.054080929 46 | 5.774662764 47 | 6.59532541 48 | 7.536085028 49 | 8.6069497 50 | 9.827935588 51 | 11.22906693 52 | 12.82535586 53 | 14.64682661 54 | 16.73351151 55 | 19.1154348 56 | 21.8326288 57 | 24.94013797 58 | 28.4880027 59 | 32.53627148 60 | 37.17001301 61 | 42.4542798 62 | 48.49415671 63 | 55.3947286 64 | 63.27609242 65 | 72.27836132 66 | 82.56666864 67 | 94.31115175 68 | 107.7319884 69 | 123.0593646 70 | 140.5685023 71 | 160.5646483 72 | 183.4130973 73 | 209.5091686 74 | 239.318238 75 | 273.3657298 76 | 312.2621367 77 | 356.6930126 78 | 407.4389875 79 | 465.410797 80 | 531.6342693 81 | 607.2753458 82 | 693.6801135 83 | 792.3748048 84 | 905.1158378 85 | 1033.899825 86 | 1181.003604 87 | 1349.039285 88 | 1540.979267 89 | 1760.236307 90 | 2010.683531 91 | 2296.769532 92 | 2623.558398 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_sim_gyr_x.txt: -------------------------------------------------------------------------------- 1 | 43652.54343 2 | 30867.00851 3 | 25202.80666 4 | 21826.27072 5 | 19522.00996 6 | 17821.07535 7 | 16499.10975 8 | 15433.50397 9 | 14550.84708 10 | 13804.1456 11 | 13161.73641 12 | 12107.03662 13 | 11271.03772 14 | 10587.29642 15 | 10014.57984 16 | 9306.753141 17 | 8730.508343 18 | 8249.554984 19 | 7716.752113 20 | 7176.433729 21 | 6735.733525 22 | 6300.701811 23 | 5886.107688 24 | 5543.878534 25 | 5180.60381 26 | 4850.282673 27 | 4526.558009 28 | 4239.909999 29 | 3968.41326 30 | 3715.951542 31 | 3483.852567 32 | 3253.668873 33 | 3048.826333 34 | 2853.656553 35 | 2671.492863 36 | 2499.538033 37 | 2336.667832 38 | 2188.105134 39 | 2046.463752 40 | 1914.291731 41 | 1791.086749 42 | 1676.467173 43 | 1569.05978 44 | 1468.195616 45 | 1373.565811 46 | 1285.012588 47 | 1202.410439 48 | 1124.859166 49 | 1052.559499 50 | 985.0095421 51 | 921.5101702 52 | 862.2589948 53 | 806.8657485 54 | 754.8841014 55 | 706.2894481 56 | 660.8806821 57 | 618.3413346 58 | 578.5605471 59 | 541.3755836 60 | 506.5120511 61 | 473.9473351 62 | 443.457786 63 | 414.9262529 64 | 388.2362681 65 | 363.2671733 66 | 339.8968605 67 | 318.0488436 68 | 297.6033668 69 | 278.4838657 70 | 260.6019247 71 | 243.8846838 72 | 228.252267 73 | 213.6463505 74 | 200.0047556 75 | 187.2743342 76 | 175.4047061 77 | 164.3563322 78 | 154.0969926 79 | 144.5997758 80 | 135.8515236 81 | 127.8523759 82 | 120.6168842 83 | 114.1804668 84 | 108.6016833 85 | 103.9678565 86 | 100.3973512 87 | 98.03950123 88 | 97.07085906 89 | 97.6852249 90 | 100.0800239 91 | 104.4430327 92 | 110.9448183 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_sim_gyr_y.txt: -------------------------------------------------------------------------------- 1 | 44700.28502 2 | 30997.12846 3 | 25140.63631 4 | 21699.14291 5 | 19368.87131 6 | 17657.24434 7 | 16331.5212 8 | 15265.56958 9 | 14384.32278 10 | 13639.94607 11 | 13000.32477 12 | 11951.69063 13 | 11121.73001 14 | 10443.67989 15 | 9876.219857 16 | 9175.468846 17 | 8605.428177 18 | 8129.939044 19 | 7603.475279 20 | 7069.873307 21 | 6634.852172 22 | 6205.592317 23 | 5796.642813 24 | 5459.172517 25 | 5101.041895 26 | 4775.476242 27 | 4456.47996 28 | 4174.071712 29 | 3906.634012 30 | 3657.982731 31 | 3429.416055 32 | 3202.762013 33 | 3001.081977 34 | 2808.943649 35 | 2629.625235 36 | 2460.370202 37 | 2300.069811 38 | 2153.862202 39 | 2014.476443 40 | 1884.418884 41 | 1763.194044 42 | 1650.425429 43 | 1544.761265 44 | 1445.543026 45 | 1352.466621 46 | 1265.376695 47 | 1184.149685 48 | 1107.900429 49 | 1036.826451 50 | 970.4345631 51 | 908.0381369 52 | 849.8320034 53 | 795.4334803 54 | 744.405327 55 | 696.7247359 56 | 652.1958941 57 | 610.5104278 58 | 571.5621584 59 | 535.1946416 60 | 501.1432341 61 | 469.3903399 62 | 439.7232625 63 | 412.034777 64 | 386.2200201 65 | 362.1722127 66 | 339.7859522 67 | 319.0026741 68 | 299.7262836 69 | 281.9062761 70 | 265.4862476 71 | 250.4304339 72 | 236.704075 73 | 224.299803 74 | 213.2156471 75 | 203.4667401 76 | 195.0802149 77 | 188.1004234 78 | 182.5839141 79 | 178.5960245 80 | 176.2118013 81 | 175.5105502 82 | 176.5729563 83 | 179.4800043 84 | 184.3137503 85 | 191.1597732 86 | 200.1122581 87 | 211.2819374 88 | 224.8027703 89 | 240.8419732 90 | 259.6051444 91 | 281.3472948 92 | 306.3774345 93 | -------------------------------------------------------------------------------- /src/imu_utils/data/data_A3_sim_gyr_z.txt: -------------------------------------------------------------------------------- 1 | 44641.86831 2 | 31566.46104 3 | 25773.89821 4 | 22320.8594 5 | 19964.39748 6 | 18224.93352 7 | 16873.02552 8 | 15783.28573 9 | 14880.63973 10 | 14117.02918 11 | 13460.07356 12 | 12381.49376 13 | 11526.56882 14 | 10827.3502 15 | 10241.67094 16 | 9517.825708 17 | 8928.542287 18 | 8436.708214 19 | 7891.853867 20 | 7339.316917 21 | 6888.653517 22 | 6443.789593 23 | 6019.828361 24 | 5669.869913 25 | 5298.393989 26 | 4960.619301 27 | 4629.594113 28 | 4336.485933 29 | 4058.874948 30 | 3800.732401 31 | 3563.415697 32 | 3328.062985 33 | 3118.626258 34 | 2919.085401 35 | 2732.848533 36 | 2557.055939 37 | 2390.558539 38 | 2238.695366 39 | 2093.916047 40 | 1958.825301 41 | 1832.909618 42 | 1715.778813 43 | 1606.029491 44 | 1502.97819 45 | 1406.309458 46 | 1315.862172 47 | 1231.508067 48 | 1152.327853 49 | 1078.526459 50 | 1009.591411 51 | 944.8093217 52 | 884.3818373 53 | 827.9108051 54 | 774.9412647 55 | 725.4482067 56 | 679.2267097 57 | 635.9546341 58 | 595.5192564 59 | 557.7551248 60 | 522.3837032 61 | 489.3822027 62 | 458.5241348 63 | 429.691339 64 | 402.7666603 65 | 377.6291775 66 | 354.157199 67 | 332.2756508 68 | 311.86712 69 | 292.8588667 70 | 275.1681952 71 | 258.7303969 72 | 243.4773548 73 | 229.3667401 74 | 216.3586563 75 | 204.4302632 76 | 193.5724937 77 | 183.8008922 78 | 175.1560517 79 | 167.7065152 80 | 161.5602436 81 | 156.8668517 82 | 153.8191952 83 | 152.6536012 84 | 153.6419277 85 | 157.0794184 86 | 163.2691527 87 | 172.5107368 88 | 185.095761 89 | 201.3163133 90 | 221.4778835 91 | 245.9228703 92 | 275.048069 93 | -------------------------------------------------------------------------------- /src/imu_utils/figure/acc.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosu12/imu_data_simulation/6fdcd99a58dda3f0b5c68e60b575d0b407e9d0e8/src/imu_utils/figure/acc.jpg -------------------------------------------------------------------------------- /src/imu_utils/figure/gyr.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosu12/imu_data_simulation/6fdcd99a58dda3f0b5c68e60b575d0b407e9d0e8/src/imu_utils/figure/gyr.jpg -------------------------------------------------------------------------------- /src/imu_utils/launch/16448.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/imu_utils/launch/A3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/imu_utils/launch/gx4.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/imu_utils/launch/n3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/imu_utils/launch/sim_imu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/imu_utils/launch/tum.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/imu_utils/launch/xsens.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/imu_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu_utils 4 | 0.1.0 5 | The imu_utils package 6 | 7 | 8 | 9 | 10 | gaowenliang 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | gaowenliang 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | catkin 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /src/imu_utils/scripts/all_of_draw_allan.m: -------------------------------------------------------------------------------- 1 | clear 2 | close all 3 | 4 | dt = dlmread('../data/data_gx4_gyr_t.txt'); 5 | data_x = dlmread('../data/data_gx4_gyr_x.txt'); 6 | data_y= dlmread('../data/data_gx4_gyr_y.txt'); 7 | data_z = dlmread('../data/data_gx4_gyr_z.txt'); 8 | data_draw=[data_x data_y data_z] ; 9 | 10 | data_sim_x= dlmread('../data/data_gx4_sim_gyr_x.txt'); 11 | data_sim_y= dlmread('../data/data_gx4_sim_gyr_y.txt'); 12 | data_sim_z= dlmread('../data/data_gx4_sim_gyr_z.txt'); 13 | data_sim_draw=[data_sim_x data_sim_y data_sim_z] ; 14 | 15 | 16 | figure 17 | loglog(dt, data_draw , 'r+'); 18 | % loglog(dt, data_sim_draw , '-'); 19 | xlabel('time:sec'); 20 | ylabel('Sigma:deg/h'); 21 | % legend('x','y','z'); 22 | grid on; 23 | hold on; 24 | loglog(dt, data_sim_draw , 'r-'); 25 | 26 | dt = dlmread('../data/data_16448_gyr_t.txt'); 27 | data_x = dlmread('../data/data_16448_gyr_x.txt'); 28 | data_y = dlmread('../data/data_16448_gyr_y.txt'); 29 | data_z = dlmread('../data/data_16448_gyr_z.txt'); 30 | data_draw=[data_x data_y data_z] ; 31 | data_sim_x= dlmread('../data/data_16448_sim_gyr_x.txt'); 32 | data_sim_y= dlmread('../data/data_16448_sim_gyr_y.txt'); 33 | data_sim_z= dlmread('../data/data_16448_sim_gyr_z.txt'); 34 | data_sim_draw=[data_sim_x data_sim_y data_sim_z] ; 35 | loglog(dt, data_draw , 'b+'); 36 | xlabel('time:sec'); 37 | loglog(dt, data_sim_draw , 'b-'); 38 | 39 | dt = dlmread('../data/data_A3_gyr_t.txt'); 40 | data_x = dlmread('../data/data_A3_gyr_x.txt'); 41 | data_y = dlmread('../data/data_A3_gyr_y.txt'); 42 | data_z = dlmread('../data/data_A3_gyr_z.txt'); 43 | data_draw=[data_x data_y data_z] ; 44 | data_sim_x= dlmread('../data/data_A3_sim_gyr_x.txt'); 45 | data_sim_y= dlmread('../data/data_A3_sim_gyr_y.txt'); 46 | data_sim_z= dlmread('../data/data_A3_sim_gyr_z.txt'); 47 | data_sim_draw=[data_sim_x data_sim_y data_sim_z] ; 48 | loglog(dt, data_draw , 'g+'); 49 | xlabel('time:sec'); 50 | loglog(dt, data_sim_draw , 'g-'); 51 | 52 | dt = dlmread('../data/data_N3_gyr_t.txt'); 53 | data_x = dlmread('../data/data_N3_gyr_x.txt'); 54 | data_y = dlmread('../data/data_N3_gyr_y.txt'); 55 | data_z = dlmread('../data/data_N3_gyr_z.txt'); 56 | data_draw=[data_x data_y data_z] ; 57 | data_sim_x= dlmread('../data/data_N3_sim_gyr_x.txt'); 58 | data_sim_y= dlmread('../data/data_N3_sim_gyr_y.txt'); 59 | data_sim_z= dlmread('../data/data_N3_sim_gyr_z.txt'); 60 | data_sim_draw=[data_sim_x data_sim_y data_sim_z] ; 61 | loglog(dt, data_draw , 'k+'); 62 | xlabel('time:sec'); 63 | loglog(dt, data_sim_draw , 'k-'); -------------------------------------------------------------------------------- /src/imu_utils/scripts/all_of_draw_allan_acc.m: -------------------------------------------------------------------------------- 1 | clear 2 | close all 3 | 4 | dt = dlmread('../data/data_gx4_acc_t.txt'); 5 | data_x = dlmread('../data/data_gx4_acc_x.txt'); 6 | data_y= dlmread('../data/data_gx4_acc_y.txt'); 7 | data_z = dlmread('../data/data_gx4_acc_z.txt'); 8 | data_draw=[data_x data_y data_z] ; 9 | 10 | data_sim_x= dlmread('../data/data_gx4_sim_acc_x.txt'); 11 | data_sim_y= dlmread('../data/data_gx4_sim_acc_y.txt'); 12 | data_sim_z= dlmread('../data/data_gx4_sim_acc_z.txt'); 13 | data_sim_draw=[data_sim_x data_sim_y data_sim_z] ; 14 | 15 | 16 | figure 17 | loglog(dt, data_draw , 'r+'); 18 | % loglog(dt, data_sim_draw , '-'); 19 | xlabel('time:sec'); 20 | ylabel('Sigma:deg/h'); 21 | % legend('x','y','z'); 22 | grid on; 23 | hold on; 24 | loglog(dt, data_sim_draw , 'r-'); 25 | 26 | dt = dlmread('../data/data_BMI160_acc_t.txt'); 27 | data_x = dlmread('../data/data_BMI160_acc_x.txt'); 28 | data_y = dlmread('../data/data_BMI160_acc_y.txt'); 29 | data_z = dlmread('../data/data_BMI160_acc_z.txt'); 30 | data_draw=[data_x data_y data_z] ; 31 | data_sim_x= dlmread('../data/data_BMI160_sim_acc_x.txt'); 32 | data_sim_y= dlmread('../data/data_BMI160_sim_acc_y.txt'); 33 | data_sim_z= dlmread('../data/data_BMI160_sim_acc_z.txt'); 34 | data_sim_draw=[data_sim_x data_sim_y data_sim_z] ; 35 | loglog(dt, data_draw , '-'); 36 | xlabel('time:sec'); 37 | loglog(dt, data_sim_draw , '-'); 38 | 39 | 40 | dt = dlmread('../data/data_16448_acc_t.txt'); 41 | data_x = dlmread('../data/data_16448_acc_x.txt'); 42 | data_y = dlmread('../data/data_16448_acc_y.txt'); 43 | data_z = dlmread('../data/data_16448_acc_z.txt'); 44 | data_draw=[data_x data_y data_z] ; 45 | data_sim_x= dlmread('../data/data_16448_sim_acc_x.txt'); 46 | data_sim_y= dlmread('../data/data_16448_sim_acc_y.txt'); 47 | data_sim_z= dlmread('../data/data_16448_sim_acc_z.txt'); 48 | data_sim_draw=[data_sim_x data_sim_y data_sim_z] ; 49 | loglog(dt, data_draw , 'b+'); 50 | xlabel('time:sec'); 51 | loglog(dt, data_sim_draw , 'b-'); 52 | 53 | dt = dlmread('../data/data_A3_acc_t.txt'); 54 | data_x = dlmread('../data/data_A3_acc_x.txt'); 55 | data_y = dlmread('../data/data_A3_acc_y.txt'); 56 | data_z = dlmread('../data/data_A3_acc_z.txt'); 57 | data_draw=[data_x data_y data_z] ; 58 | data_sim_x= dlmread('../data/data_A3_sim_acc_x.txt'); 59 | data_sim_y= dlmread('../data/data_A3_sim_acc_y.txt'); 60 | data_sim_z= dlmread('../data/data_A3_sim_acc_z.txt'); 61 | data_sim_draw=[data_sim_x data_sim_y data_sim_z] ; 62 | loglog(dt, data_draw , 'g+'); 63 | xlabel('time:sec'); 64 | loglog(dt, data_sim_draw , 'g-'); 65 | 66 | dt = dlmread('../data/data_N3_acc_t.txt'); 67 | data_x = dlmread('../data/data_N3_acc_x.txt'); 68 | data_y = dlmread('../data/data_N3_acc_y.txt'); 69 | data_z = dlmread('../data/data_N3_acc_z.txt'); 70 | data_draw=[data_x data_y data_z] ; 71 | data_sim_x= dlmread('../data/data_N3_sim_acc_x.txt'); 72 | data_sim_y= dlmread('../data/data_N3_sim_acc_y.txt'); 73 | data_sim_z= dlmread('../data/data_N3_sim_acc_z.txt'); 74 | data_sim_draw=[data_sim_x data_sim_y data_sim_z] ; 75 | loglog(dt, data_draw , 'k+'); 76 | xlabel('time:sec'); 77 | loglog(dt, data_sim_draw , 'k-'); 78 | 79 | 80 | dt = dlmread('../data/data_xsens_acc_t.txt'); 81 | data_x = dlmread('../data/data_xsens_acc_x.txt'); 82 | data_y = dlmread('../data/data_xsens_acc_y.txt'); 83 | data_z = dlmread('../data/data_xsens_acc_z.txt'); 84 | data_draw=[data_x data_y data_z]; 85 | data_sim_x= dlmread('../data/data_xsens_sim_acc_x.txt'); 86 | data_sim_y= dlmread('../data/data_xsens_sim_acc_y.txt'); 87 | data_sim_z= dlmread('../data/data_xsens_sim_acc_z.txt'); 88 | data_sim_draw=[data_sim_x data_sim_y data_sim_z]; 89 | loglog(dt, data_draw , 'o'); 90 | xlabel('time:sec'); 91 | loglog(dt, data_sim_draw , '-'); -------------------------------------------------------------------------------- /src/imu_utils/scripts/allan_with_degree.m: -------------------------------------------------------------------------------- 1 | clear 2 | close all 3 | 4 | dt = dlmread('../data/data_gx4_gyr_t.txt'); 5 | data_x = dlmread('../data/data_gx4_gyr_x.txt'); 6 | data_y= dlmread('../data/data_gx4_gyr_y.txt'); 7 | data_z = dlmread('../data/data_gx4_gyr_z.txt'); 8 | data_draw=(data_x+data_y+data_z)/3 /3600; 9 | 10 | data_sim_x= dlmread('../data/data_gx4_sim_gyr_x.txt'); 11 | data_sim_y= dlmread('../data/data_gx4_sim_gyr_y.txt'); 12 | data_sim_z= dlmread('../data/data_gx4_sim_gyr_z.txt'); 13 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3 /3600; 14 | 15 | figure 16 | loglog(dt, data_draw , 'r+'); 17 | % loglog(dt, data_sim_draw , '-'); 18 | xlabel('time:sec'); 19 | ylabel('Sigma:deg/s'); 20 | % legend('x','y','z'); 21 | grid on; 22 | hold on; 23 | loglog(dt, data_sim_draw , 'r-'); 24 | 25 | dt = dlmread('../data/data_BMI160_gyr_t.txt'); 26 | data_x = dlmread('../data/data_BMI160_gyr_x.txt'); 27 | data_y = dlmread('../data/data_BMI160_gyr_y.txt'); 28 | data_z = dlmread('../data/data_BMI160_gyr_z.txt'); 29 | data_draw=(data_x+data_y+data_z)/3 /3600; 30 | data_sim_x= dlmread('../data/data_BMI160_sim_gyr_x.txt'); 31 | data_sim_y= dlmread('../data/data_BMI160_sim_gyr_y.txt'); 32 | data_sim_z= dlmread('../data/data_BMI160_sim_gyr_z.txt'); 33 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3 /3600; 34 | loglog(dt, data_draw , '-'); 35 | xlabel('time:sec'); 36 | loglog(dt, data_sim_draw , '-'); 37 | 38 | 39 | dt = dlmread('../data/data_16448_gyr_t.txt'); 40 | data_x = dlmread('../data/data_16448_gyr_x.txt'); 41 | data_y = dlmread('../data/data_16448_gyr_y.txt'); 42 | data_z = dlmread('../data/data_16448_gyr_z.txt'); 43 | data_draw=(data_x+data_y+data_z)/3/3600; 44 | data_sim_x= dlmread('../data/data_16448_sim_gyr_x.txt'); 45 | data_sim_y= dlmread('../data/data_16448_sim_gyr_y.txt'); 46 | data_sim_z= dlmread('../data/data_16448_sim_gyr_z.txt'); 47 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3/3600; 48 | loglog(dt, data_draw , 'b+'); 49 | xlabel('time:sec'); 50 | loglog(dt, data_sim_draw , 'b-'); 51 | 52 | dt = dlmread('../data/data_A3_gyr_t.txt'); 53 | data_x = dlmread('../data/data_A3_gyr_x.txt'); 54 | data_y = dlmread('../data/data_A3_gyr_y.txt'); 55 | data_z = dlmread('../data/data_A3_gyr_z.txt'); 56 | data_draw=(data_x+data_y+data_z)/3/3600; 57 | data_sim_x= dlmread('../data/data_A3_sim_gyr_x.txt'); 58 | data_sim_y= dlmread('../data/data_A3_sim_gyr_y.txt'); 59 | data_sim_z= dlmread('../data/data_A3_sim_gyr_z.txt'); 60 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3/3600; 61 | loglog(dt, data_draw , 'g+'); 62 | xlabel('time:sec'); 63 | loglog(dt, data_sim_draw , 'g-'); 64 | 65 | dt = dlmread('../data/data_N3_gyr_t.txt'); 66 | data_x = dlmread('../data/data_N3_gyr_x.txt'); 67 | data_y = dlmread('../data/data_N3_gyr_y.txt'); 68 | data_z = dlmread('../data/data_N3_gyr_z.txt'); 69 | data_draw=(data_x+data_y+data_z)/3/3600; 70 | data_sim_x= dlmread('../data/data_N3_sim_gyr_x.txt'); 71 | data_sim_y= dlmread('../data/data_N3_sim_gyr_y.txt'); 72 | data_sim_z= dlmread('../data/data_N3_sim_gyr_z.txt'); 73 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3/3600; 74 | loglog(dt, data_draw , 'k+'); 75 | xlabel('time:sec'); 76 | loglog(dt, data_sim_draw , 'k-'); -------------------------------------------------------------------------------- /src/imu_utils/scripts/avg_of_all_sensors.m: -------------------------------------------------------------------------------- 1 | clear 2 | close all 3 | 4 | dt = dlmread('../data/data_gx4_gyr_t.txt'); 5 | data_x = dlmread('../data/data_gx4_gyr_x.txt'); 6 | data_y= dlmread('../data/data_gx4_gyr_y.txt'); 7 | data_z = dlmread('../data/data_gx4_gyr_z.txt'); 8 | data_draw=(data_x+data_y+data_z)/3 ; 9 | 10 | data_sim_x= dlmread('../data/data_gx4_sim_gyr_x.txt'); 11 | data_sim_y= dlmread('../data/data_gx4_sim_gyr_y.txt'); 12 | data_sim_z= dlmread('../data/data_gx4_sim_gyr_z.txt'); 13 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3 ; 14 | 15 | figure 16 | loglog(dt, data_draw , 'r+'); 17 | % loglog(dt, data_sim_draw , '-'); 18 | xlabel('time:sec'); 19 | ylabel('Sigma:deg/h'); 20 | % legend('x','y','z'); 21 | grid on; 22 | hold on; 23 | loglog(dt, data_sim_draw , 'r-'); 24 | 25 | dt = dlmread('../data/data_16448_gyr_t.txt'); 26 | data_x = dlmread('../data/data_16448_gyr_x.txt'); 27 | data_y = dlmread('../data/data_16448_gyr_y.txt'); 28 | data_z = dlmread('../data/data_16448_gyr_z.txt'); 29 | data_draw=(data_x+data_y+data_z)/3 ; 30 | data_sim_x= dlmread('../data/data_16448_sim_gyr_x.txt'); 31 | data_sim_y= dlmread('../data/data_16448_sim_gyr_y.txt'); 32 | data_sim_z= dlmread('../data/data_16448_sim_gyr_z.txt'); 33 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3 ; 34 | loglog(dt, data_draw , 'b+'); 35 | xlabel('time:sec'); 36 | loglog(dt, data_sim_draw , 'b-'); 37 | 38 | dt = dlmread('../data/data_A3_gyr_t.txt'); 39 | data_x = dlmread('../data/data_A3_gyr_x.txt'); 40 | data_y = dlmread('../data/data_A3_gyr_y.txt'); 41 | data_z = dlmread('../data/data_A3_gyr_z.txt'); 42 | data_draw=(data_x+data_y+data_z)/3; 43 | data_sim_x= dlmread('../data/data_A3_sim_gyr_x.txt'); 44 | data_sim_y= dlmread('../data/data_A3_sim_gyr_y.txt'); 45 | data_sim_z= dlmread('../data/data_A3_sim_gyr_z.txt'); 46 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3; 47 | loglog(dt, data_draw , 'g+'); 48 | xlabel('time:sec'); 49 | loglog(dt, data_sim_draw , 'g-'); 50 | 51 | dt = dlmread('../data/data_N3_gyr_t.txt'); 52 | data_x = dlmread('../data/data_N3_gyr_x.txt'); 53 | data_y = dlmread('../data/data_N3_gyr_y.txt'); 54 | data_z = dlmread('../data/data_N3_gyr_z.txt'); 55 | data_draw=(data_x+data_y+data_z)/3; 56 | data_sim_x= dlmread('../data/data_N3_sim_gyr_x.txt'); 57 | data_sim_y= dlmread('../data/data_N3_sim_gyr_y.txt'); 58 | data_sim_z= dlmread('../data/data_N3_sim_gyr_z.txt'); 59 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3; 60 | loglog(dt, data_draw , 'k+'); 61 | xlabel('time:sec'); 62 | loglog(dt, data_sim_draw , 'k-'); 63 | 64 | 65 | dt = dlmread('../data/data_xsens_gyr_t.txt'); 66 | data_x = dlmread('../data/data_xsens_gyr_x.txt'); 67 | data_y = dlmread('../data/data_xsens_gyr_y.txt'); 68 | data_z = dlmread('../data/data_xsens_gyr_z.txt'); 69 | data_draw=(data_x+data_y+data_z)/3; 70 | data_sim_x= dlmread('../data/data_xsens_sim_gyr_x.txt'); 71 | data_sim_y= dlmread('../data/data_xsens_sim_gyr_y.txt'); 72 | data_sim_z= dlmread('../data/data_xsens_sim_gyr_z.txt'); 73 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3; 74 | loglog(dt, data_draw , 'o'); 75 | xlabel('time:sec'); 76 | loglog(dt, data_sim_draw , '-'); -------------------------------------------------------------------------------- /src/imu_utils/scripts/avg_of_all_sensors_acc.m: -------------------------------------------------------------------------------- 1 | clear 2 | close all 3 | 4 | dt = dlmread('../data/data_gx4_acc_t.txt'); 5 | data_x = dlmread('../data/data_gx4_acc_x.txt'); 6 | data_y= dlmread('../data/data_gx4_acc_y.txt'); 7 | data_z = dlmread('../data/data_gx4_acc_z.txt'); 8 | data_draw=(data_x+data_y+data_z)/3 ; 9 | 10 | data_sim_x= dlmread('../data/data_gx4_sim_acc_x.txt'); 11 | data_sim_y= dlmread('../data/data_gx4_sim_acc_y.txt'); 12 | data_sim_z= dlmread('../data/data_gx4_sim_acc_z.txt'); 13 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3 ; 14 | 15 | figure 16 | loglog(dt, data_draw , 'r+'); 17 | % loglog(dt, data_sim_draw , '-'); 18 | xlabel('time:sec'); 19 | ylabel('Sigma'); 20 | % legend('x','y','z'); 21 | grid on; 22 | hold on; 23 | loglog(dt, data_sim_draw , 'r-'); 24 | 25 | dt = dlmread('../data/data_16448_acc_t.txt'); 26 | data_x = dlmread('../data/data_16448_acc_x.txt'); 27 | data_y = dlmread('../data/data_16448_acc_y.txt'); 28 | data_z = dlmread('../data/data_16448_acc_z.txt'); 29 | data_draw=(data_x+data_y+data_z)/3 ; 30 | data_sim_x= dlmread('../data/data_16448_sim_acc_x.txt'); 31 | data_sim_y= dlmread('../data/data_16448_sim_acc_y.txt'); 32 | data_sim_z= dlmread('../data/data_16448_sim_acc_z.txt'); 33 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3 ; 34 | loglog(dt, data_draw , 'b+'); 35 | xlabel('time:sec'); 36 | loglog(dt, data_sim_draw , 'b-'); 37 | 38 | 39 | dt = dlmread('../data/data_BMI160_acc_t.txt'); 40 | data_x = dlmread('../data/data_BMI160_acc_x.txt'); 41 | data_y = dlmread('../data/data_BMI160_acc_y.txt'); 42 | data_z = dlmread('../data/data_BMI160_acc_z.txt'); 43 | data_draw=(data_x+data_y+data_z)/3 ; 44 | data_sim_x= dlmread('../data/data_BMI160_sim_acc_x.txt'); 45 | data_sim_y= dlmread('../data/data_BMI160_sim_acc_y.txt'); 46 | data_sim_z= dlmread('../data/data_BMI160_sim_acc_z.txt'); 47 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3 ; 48 | loglog(dt, data_draw , 'b+'); 49 | xlabel('time:sec'); 50 | loglog(dt, data_sim_draw , 'b-'); 51 | 52 | dt = dlmread('../data/data_A3_acc_t.txt'); 53 | data_x = dlmread('../data/data_A3_acc_x.txt'); 54 | data_y = dlmread('../data/data_A3_acc_y.txt'); 55 | data_z = dlmread('../data/data_A3_acc_z.txt'); 56 | data_draw=(data_x+data_y+data_z)/3; 57 | data_sim_x= dlmread('../data/data_A3_sim_acc_x.txt'); 58 | data_sim_y= dlmread('../data/data_A3_sim_acc_y.txt'); 59 | data_sim_z= dlmread('../data/data_A3_sim_acc_z.txt'); 60 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3; 61 | loglog(dt, data_draw , 'g+'); 62 | xlabel('time:sec'); 63 | loglog(dt, data_sim_draw , 'g-'); 64 | 65 | dt = dlmread('../data/data_N3_acc_t.txt'); 66 | data_x = dlmread('../data/data_N3_acc_x.txt'); 67 | data_y = dlmread('../data/data_N3_acc_y.txt'); 68 | data_z = dlmread('../data/data_N3_acc_z.txt'); 69 | data_draw=(data_x+data_y+data_z)/3; 70 | data_sim_x= dlmread('../data/data_N3_sim_acc_x.txt'); 71 | data_sim_y= dlmread('../data/data_N3_sim_acc_y.txt'); 72 | data_sim_z= dlmread('../data/data_N3_sim_acc_z.txt'); 73 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3; 74 | loglog(dt, data_draw , 'k+'); 75 | xlabel('time:sec'); 76 | loglog(dt, data_sim_draw , 'k-'); 77 | 78 | 79 | 80 | dt = dlmread('../data/data_xsens_acc_t.txt'); 81 | data_x = dlmread('../data/data_xsens_acc_x.txt'); 82 | data_y = dlmread('../data/data_xsens_acc_y.txt'); 83 | data_z = dlmread('../data/data_xsens_acc_z.txt'); 84 | data_draw=(data_x+data_y+data_z)/3; 85 | data_sim_x= dlmread('../data/data_xsens_sim_acc_x.txt'); 86 | data_sim_y= dlmread('../data/data_xsens_sim_acc_y.txt'); 87 | data_sim_z= dlmread('../data/data_xsens_sim_acc_z.txt'); 88 | data_sim_draw=(data_sim_x+data_sim_y+data_sim_z)/3; 89 | loglog(dt, data_draw , 'o'); 90 | xlabel('time:sec'); 91 | loglog(dt, data_sim_draw , '-'); -------------------------------------------------------------------------------- /src/imu_utils/scripts/draw_allan.m: -------------------------------------------------------------------------------- 1 | clear 2 | close all 3 | 4 | dt = dlmread('../data/data_A3_gyr_t.txt'); 5 | data_x = dlmread('../data/data_A3_gyr_x.txt'); 6 | data_y= dlmread('../data/data_A3_gyr_y.txt'); 7 | data_z = dlmread('../data/data_A3_gyr_z.txt'); 8 | data_draw=[data_x data_y data_z] ; 9 | 10 | data_sim_x= dlmread('../data/data_A3_sim_gyr_x.txt'); 11 | data_sim_y= dlmread('../data/data_A3_sim_gyr_y.txt'); 12 | data_sim_z= dlmread('../data/data_A3_sim_gyr_z.txt'); 13 | data_sim_draw=[data_sim_x data_sim_y data_sim_z] ; 14 | 15 | 16 | figure 17 | loglog(dt, data_draw , 'o'); 18 | % loglog(dt, data_sim_draw , '-'); 19 | xlabel('time:sec'); 20 | ylabel('Sigma:deg/h'); 21 | % legend('x','y','z'); 22 | grid on; 23 | hold on; 24 | loglog(dt, data_sim_draw , '-'); 25 | -------------------------------------------------------------------------------- /src/imu_utils/scripts/ideal_allan.m: -------------------------------------------------------------------------------- 1 | clc 2 | close all 3 | clear 4 | 5 | size = 1000000; 6 | a_tmp = linspace(0,1000,size)'; 7 | a = a_tmp(2:size,:); 8 | b = a.^(-2)+a.^(-1)+1+a.^(1)+a.^(2); 9 | 10 | figure 11 | loglog(a, sqrt(b) , '-'); 12 | hold on; 13 | grid on; 14 | mea([1:size-1],1)= mean(b); 15 | loglog(a, sqrt(mea) , '-'); 16 | 17 | -------------------------------------------------------------------------------- /src/imu_utils/src/acc_lib/allan_acc.cpp: -------------------------------------------------------------------------------- 1 | #include "allan_acc.h" 2 | 3 | imu::AllanAcc::AllanAcc( std::string name, int maxCluster ) 4 | : m_name( name ) 5 | , numData( 0 ) 6 | , numCluster( maxCluster ) 7 | { 8 | std::cout << m_name << " " 9 | << " num of Cluster " << numCluster << std::endl; 10 | } 11 | 12 | imu::AllanAcc::~AllanAcc( ) 13 | { 14 | m_rawData.clear( ); 15 | m_thetas.clear( ); 16 | mFactors.clear( ); 17 | } 18 | 19 | void 20 | imu::AllanAcc::pushRadPerSec( double data, double time ) 21 | { 22 | m_rawData.push_back( AccData( data * 57.3 * 3600, time ) ); 23 | numData++; 24 | } 25 | 26 | void 27 | imu::AllanAcc::pushDegreePerSec( double data, double time ) 28 | { 29 | m_rawData.push_back( AccData( data * 3600, time ) ); 30 | numData++; 31 | } 32 | 33 | void 34 | imu::AllanAcc::pushMPerSec2( double data, double time ) 35 | { 36 | m_rawData.push_back( AccData( data, time ) ); 37 | numData++; 38 | } 39 | 40 | void 41 | imu::AllanAcc::calc( ) 42 | { 43 | std::cout << m_name << " " 44 | << " numData " << numData << std::endl; 45 | if ( numData < 10000 ) 46 | std::cout << m_name << " " 47 | << " Too few number" << std::endl; 48 | 49 | double start_t = m_rawData.begin( )->t; 50 | double end_t = m_rawData[numData - 1].t; 51 | std::cout << m_name << " " 52 | << " start_t " << start_t << std::endl; 53 | std::cout << m_name << " " 54 | << " end_t " << end_t << std::endl; 55 | std::cout << m_name << " " 56 | << "dt " << std::endl // 57 | << "-------------" << ( end_t - start_t ) << " s" << std::endl 58 | << "-------------" << ( end_t - start_t ) / 60 << " min" << std::endl 59 | << "-------------" << ( end_t - start_t ) / 3600 << " h" << std::endl; 60 | 61 | if ( ( end_t - start_t ) / 60 < 10 ) 62 | std::cout << m_name << " " 63 | << " Too short time!!!!" << std::endl; 64 | 65 | m_freq = getAvgFreq( ); 66 | std::cout << m_name << " " 67 | << " freq " << m_freq << std::endl; 68 | 69 | double period = getAvgPeriod( ); 70 | std::cout << m_name << " " 71 | << " period " << period << std::endl; 72 | 73 | m_thetas = calcThetas( m_freq ); 74 | 75 | initStrides( ); 76 | 77 | mVariance = calcVariance( period ); 78 | } 79 | 80 | std::vector< double > 81 | imu::AllanAcc::getVariance( ) const 82 | { 83 | return mVariance; 84 | } 85 | 86 | std::vector< double > 87 | imu::AllanAcc::getDeviation( ) 88 | { 89 | double period = getAvgPeriod( ); 90 | 91 | std::vector< double > sigma2 = calcVariance( period ); 92 | std::vector< double > sigma; 93 | 94 | for ( auto& sig : sigma2 ) 95 | { 96 | sigma.push_back( sqrt( sig ) ); 97 | } 98 | return sigma; 99 | } 100 | 101 | std::vector< double > 102 | imu::AllanAcc::getTimes( ) 103 | { 104 | double period = getAvgPeriod( ); 105 | std::vector< double > time( numFactors, 0.0 ); 106 | for ( int i = 0; i < numFactors; i++ ) 107 | { 108 | int factor = mFactors[i]; 109 | // double clusterPeriod2 = ( period * factor ) * ( period * factor ); 110 | time[i] = period * factor; 111 | } 112 | return time; 113 | } 114 | 115 | std::vector< int > 116 | imu::AllanAcc::getFactors( ) const 117 | { 118 | return mFactors; 119 | } 120 | 121 | double 122 | imu::AllanAcc::getFreq( ) const 123 | { 124 | return m_freq; 125 | } 126 | 127 | std::vector< double > 128 | imu::AllanAcc::calcVariance( double period ) 129 | { 130 | std::vector< double > sigma2( numFactors, 0.0 ); 131 | 132 | for ( int i = 0; i < numFactors; i++ ) 133 | { 134 | int factor = mFactors[i]; 135 | double clusterPeriod2 = ( period * factor ) * ( period * factor ); 136 | double divided = 2 * clusterPeriod2 * ( numData - 2 * factor ); 137 | int max = numData - 2 * factor; 138 | 139 | for ( int k = 0; k < max; k++ ) 140 | { 141 | double temp = ( m_thetas[k + 2 * factor] - 2 * m_thetas[k + factor] + m_thetas[k] ); 142 | sigma2[i] += ( temp * temp ); 143 | } 144 | 145 | // std::cout << "sigma2 " << sigma2[i] << std::endl; 146 | sigma2[i] = sigma2[i] / divided; 147 | } 148 | 149 | // for ( int index = 0; index < mVariance.size( ); ++index ) 150 | // { 151 | // std::cout << sigma2[index] << std::endl; 152 | // } 153 | return sigma2; 154 | } 155 | 156 | std::vector< double > 157 | imu::AllanAcc::calcThetas( const double freq ) 158 | { 159 | std::vector< double > thetas; 160 | 161 | double sum = 0; 162 | for ( auto& acc : m_rawData ) 163 | { 164 | sum += acc.a; 165 | thetas.push_back( sum / freq ); 166 | } 167 | return thetas; 168 | } 169 | 170 | void 171 | imu::AllanAcc::initStrides( ) 172 | { 173 | 174 | int mode = numData / 2; 175 | unsigned int maxStride = 1; 176 | int shft = 0; 177 | while ( mode ) 178 | { 179 | mode = mode >> 1; 180 | maxStride = 1 << shft; 181 | shft++; 182 | } 183 | // std::cout << m_name << " " 184 | // << " mode: " << mode << std::endl; 185 | // std::cout << m_name << " " 186 | // << " shft: " << shft << std::endl; 187 | 188 | std::vector< double > avgFactors = getLogSpace( 0, log10( maxStride ) ); 189 | // std::cout << m_name << " " 190 | // << " avgFactors " << avgFactors.size( ) << std::endl; 191 | 192 | for ( int i = 0; i < numCluster; i++ ) 193 | { 194 | // std::cout << m_name << " " 195 | // << " avgFactors " << i << " " << avgFactors[i] << std::endl; 196 | avgFactors[i] = ceil( avgFactors[i] ); 197 | } 198 | 199 | std::vector< int > factors( numCluster, 0 ); 200 | 201 | numFactors = 1; 202 | factors[0] = ( int )avgFactors[0]; 203 | 204 | for ( int i = 1; i < numCluster; i++ ) 205 | { 206 | // std::cout << m_name << " " 207 | // << " avgFactors " << i << " " << avgFactors[i] << std::endl; 208 | if ( avgFactors[i] != avgFactors[i - 1] ) 209 | { 210 | factors[numFactors] = ( int )avgFactors[i]; 211 | numFactors++; 212 | } 213 | } 214 | 215 | mFactors = factors; 216 | 217 | // std::cout << m_name << " " 218 | // << " numFactors " << numFactors << std::endl; 219 | } 220 | 221 | std::vector< double > 222 | imu::AllanAcc::getLogSpace( float a, float b ) 223 | { 224 | std::vector< double > logSpace; 225 | 226 | double start = pow( 10, a ); 227 | double end = pow( 10, b ); 228 | double progression = pow( end / start, ( float )1 / ( numCluster - 1 ) ); 229 | 230 | logSpace.push_back( start ); 231 | for ( int i = 1; i < numCluster; i++ ) 232 | { 233 | logSpace.push_back( logSpace[i - 1] * progression ); 234 | } 235 | // std::cout << m_name << " " 236 | // << "size logSpace " << logSpace.size( ) << std::endl; 237 | return logSpace; 238 | } 239 | 240 | double 241 | imu::AllanAcc::getAvgDt( ) 242 | { 243 | double sum_dt = 0.0; 244 | double start_t = m_rawData[0].t; 245 | bool first = true; 246 | for ( auto& acc : m_rawData ) 247 | { 248 | if ( !first ) 249 | sum_dt += ( acc.t - start_t ); 250 | start_t = acc.t; 251 | first = false; 252 | } 253 | return sum_dt / ( numData - 1 ); 254 | } 255 | -------------------------------------------------------------------------------- /src/imu_utils/src/acc_lib/allan_acc.h: -------------------------------------------------------------------------------- 1 | #ifndef AllanAcc_H 2 | #define AllanAcc_H 3 | 4 | #include "../type.h" 5 | #include 6 | #include 7 | #include 8 | 9 | namespace imu 10 | { 11 | 12 | class AllanAcc 13 | { 14 | public: 15 | AllanAcc( std::string name, int maxCluster = 10000 ); 16 | ~AllanAcc( ); 17 | void pushRadPerSec( double data, double time ); 18 | void pushDegreePerSec( double data, double time ); 19 | void pushMPerSec2( double data, double time ); 20 | void calc( ); 21 | 22 | std::vector< double > getVariance( ) const; 23 | std::vector< double > getDeviation( ); 24 | std::vector< double > getTimes( ); 25 | std::vector< int > getFactors( ) const; 26 | double getFreq( ) const; 27 | 28 | private: 29 | std::vector< double > calcVariance( double period ); 30 | 31 | std::vector< double > calcThetas( const double freq ); 32 | void initStrides( ); 33 | std::vector< double > getLogSpace( float a, float b ); 34 | double getAvgFreq( ) { return 1.0 / getAvgDt( ); } 35 | double getAvgDt( ); 36 | double getAvgPeriod( ) { return getAvgDt( ); } 37 | int getFactorsNum( ) { return numFactors; } 38 | 39 | std::string m_name; 40 | double m_freq; 41 | int numData; 42 | std::vector< AccData > m_rawData; 43 | std::vector< double > m_thetas; 44 | int numCluster; 45 | int numFactors; 46 | std::vector< int > mFactors; 47 | 48 | std::vector< double > mVariance; 49 | }; 50 | } 51 | 52 | #endif // AllanAcc_H 53 | -------------------------------------------------------------------------------- /src/imu_utils/src/acc_lib/fitallan_acc.cpp: -------------------------------------------------------------------------------- 1 | #include "fitallan_acc.h" 2 | 3 | using namespace imu; 4 | 5 | FitAllanAcc::FitAllanAcc( std::vector< double > sigma2s, std::vector< double > taus, double _freq ) 6 | : Q( 0.0 ) 7 | , N( 0.0 ) 8 | , B( 0.0 ) 9 | , K( 0.0 ) 10 | , R( 0.0 ) 11 | , freq( _freq ) 12 | { 13 | if ( sigma2s.size( ) != taus.size( ) ) 14 | std::cerr << "Error of point size" << std::endl; 15 | 16 | std::vector< double > sigma2s_tmp = checkData( sigma2s, taus ); 17 | 18 | std::vector< double > init = initValue( sigma2s_tmp, m_taus ); 19 | 20 | int num_samples = sigma2s_tmp.size( ); 21 | // double param[] = { Q, N, B, K, R }; 22 | double param[] = { init[0], init[1], init[2], init[3], init[4] }; 23 | 24 | ceres::Problem problem; 25 | 26 | for ( int i = 0; i < num_samples; ++i ) 27 | { 28 | 29 | // std::cout << "sigma " << i << " " << taus[i] << " " << sigma2s[i] << 30 | // std::endl; 31 | 32 | ceres::CostFunction* f = new ceres::AutoDiffCostFunction< AllanSigmaError, 1, 5 >( 33 | new AllanSigmaError( sigma2s_tmp[i], m_taus[i] ) ); 34 | 35 | problem.AddResidualBlock( f, NULL /* squared loss */, param ); 36 | } 37 | ceres::Solver::Options options; 38 | options.minimizer_progress_to_stdout = true; 39 | options.logging_type = ceres::SILENT; 40 | options.trust_region_strategy_type = ceres::DOGLEG; 41 | // options.max_num_iterations = 5; 42 | 43 | ceres::Solver::Summary summary; 44 | ceres::Solve( options, &problem, &summary ); 45 | // std::cout << summary.FullReport( ) << "\n"; 46 | // std::cout << "num_parameters " << summary.num_parameters << std::endl; 47 | 48 | Q = param[0]; 49 | N = param[1]; 50 | B = param[2]; 51 | K = param[3]; 52 | R = param[4]; 53 | 54 | // std::cout << "Q " << Q // 55 | // << " " << N // 56 | // << " " << B // 57 | // << " " << K // 58 | // << " " << R << std::endl; 59 | 60 | std::cout << " Bias Instability " << getBiasInstability( ) << " m/s^2" << std::endl; 61 | std::cout << " White Noise " << getWhiteNoise( ) << " m/s^2" << std::endl; 62 | } 63 | 64 | std::vector< double > 65 | FitAllanAcc::initValue( std::vector< double > sigma2s, std::vector< double > taus ) 66 | { 67 | if ( sigma2s.size( ) != taus.size( ) ) 68 | std::cout << " Error with data size!!! " << sigma2s.size( ) << " " << taus.size( ) << std::endl; 69 | 70 | Eigen::MatrixXd Y( sigma2s.size( ), 1 ); 71 | 72 | for ( unsigned int index = 0; index < sigma2s.size( ); ++index ) 73 | { 74 | Y( index, 0 ) = sqrt( sigma2s[index] ); 75 | } 76 | // std::cout << "Y " << Y << std::endl; 77 | 78 | int m_order = 2; 79 | 80 | Eigen::MatrixXd B( 2 * m_order + 1, 1 ); 81 | B.setZero( ); 82 | 83 | Eigen::MatrixXd F( taus.size( ), 2 * m_order + 1 ); 84 | F.setZero( ); 85 | 86 | for ( unsigned int index = 0; index < taus.size( ); ++index ) 87 | for ( int order_index = 0; order_index < 2 * m_order + 1; ++order_index ) 88 | { 89 | int kk = order_index - m_order; 90 | F( index, order_index ) = pow( sqrt( taus[index] ), kk ); 91 | } 92 | // std::cout << "F " << F << std::endl; 93 | 94 | Eigen::MatrixXd A = F.transpose( ) * F; 95 | B = F.transpose( ) * Y; 96 | // std::cout << "B " << B << std::endl; 97 | // std::cout << "A " << A << std::endl; 98 | 99 | Eigen::MatrixXd C = A.inverse( ) * B; 100 | std::cout << "C " << C.transpose( ) << std::endl; 101 | 102 | std::vector< double > init; 103 | for ( int index = 0; index < 2 * m_order + 1; ++index ) 104 | init.push_back( std::abs( C( index, 0 ) ) ); 105 | 106 | return init; 107 | } 108 | 109 | std::vector< double > 110 | FitAllanAcc::calcSimDeviation( const std::vector< double > taus ) const 111 | { 112 | std::vector< double > des; 113 | for ( auto& tau : taus ) 114 | des.push_back( sqrt( calcSigma2( Q, N, B, K, R, tau ) ) ); 115 | return des; 116 | } 117 | 118 | double 119 | FitAllanAcc::getBiasInstability( ) const 120 | { 121 | return findMinNum( calcSimDeviation( m_taus ) ); 122 | } 123 | 124 | double 125 | FitAllanAcc::getWhiteNoise( ) const 126 | { 127 | return sqrt( freq ) * sqrt( calcSigma2( Q, N, B, K, R, 1 ) ); 128 | } 129 | 130 | std::vector< double > 131 | FitAllanAcc::checkData( std::vector< double > sigma2s, std::vector< double > taus ) 132 | { 133 | std::vector< double > sigma2s_tmp; 134 | double data_tmp = 0; 135 | // bool is_first = true; 136 | for ( unsigned int index = 0; index < sigma2s.size( ); ++index ) 137 | { 138 | if ( taus[index] < 1 ) 139 | { 140 | if ( data_tmp < sigma2s[index] ) 141 | { 142 | data_tmp = sigma2s[index]; 143 | continue; 144 | } 145 | else 146 | { 147 | sigma2s_tmp.push_back( sigma2s[index] ); 148 | m_taus.push_back( taus[index] ); 149 | } 150 | } 151 | else 152 | { 153 | sigma2s_tmp.push_back( sigma2s[index] ); 154 | m_taus.push_back( taus[index] ); 155 | } 156 | } 157 | return sigma2s_tmp; 158 | } 159 | 160 | double 161 | FitAllanAcc::findMinNum( const std::vector< double > num ) const 162 | { 163 | double min = 1000.0; 164 | for ( unsigned int index = 0; index < num.size( ); ++index ) 165 | min = min < num[index] ? min : num[index]; 166 | return min; 167 | } 168 | 169 | int 170 | FitAllanAcc::findMinIndex( std::vector< double > num ) 171 | { 172 | double min = 1000.0; 173 | int min_index = 0; 174 | for ( unsigned int index = 0; index < num.size( ); ++index ) 175 | { 176 | min_index = min < num[index] ? min_index : index; 177 | min = min < num[index] ? min : num[index]; 178 | } 179 | return min_index; 180 | } 181 | 182 | double 183 | FitAllanAcc::calcSigma2( double _Q, double _N, double _B, double _K, double _R, double _tau ) const 184 | { 185 | // clang-format off 186 | return _Q * _Q / ( _tau * _tau ) 187 | + _N * _N / _tau 188 | + _B * _B 189 | + _K * _K * _tau 190 | + _R * _R * _tau * _tau; 191 | // clang-format on 192 | } 193 | 194 | double 195 | FitAllanAcc::getN( ) const 196 | { 197 | return sqrt( N * N ) / 60.0; 198 | } 199 | 200 | double 201 | FitAllanAcc::getB( ) const 202 | { 203 | return sqrt( B * B ) / 0.6642824703; 204 | } 205 | 206 | double 207 | FitAllanAcc::getK( ) const 208 | { 209 | return 60.0 * sqrt( 3.0 * K * K ); 210 | } 211 | 212 | double 213 | FitAllanAcc::getR( ) const 214 | { 215 | return 3600.0 * sqrt( 2.0 * R * R ); 216 | } 217 | 218 | double 219 | FitAllanAcc::getQ( ) const 220 | { 221 | return sqrt( Q * Q ) / ( 3600.0 * sqrt( 3.0 ) ); 222 | } 223 | 224 | // double 225 | // FitAllanAcc::getN( ) const 226 | //{ 227 | // return sqrt( N ) / 60.0; 228 | //} 229 | 230 | // double 231 | // FitAllanAcc::getB( ) const 232 | //{ 233 | // return sqrt( B ) / 0.6642824703; 234 | //} 235 | 236 | // double 237 | // FitAllanAcc::getK( ) const 238 | //{ 239 | // return 60.0 * sqrt( 3.0 * K ); 240 | //} 241 | 242 | // double 243 | // FitAllanAcc::getR( ) const 244 | //{ 245 | // return 3600.0 * sqrt( 2.0 * R ); 246 | //} 247 | 248 | // double 249 | // FitAllanAcc::getQ( ) const 250 | //{ 251 | // return sqrt( Q ) / ( 3600.0 * sqrt( 3.0 ) ); 252 | //} 253 | -------------------------------------------------------------------------------- /src/imu_utils/src/acc_lib/fitallan_acc.h: -------------------------------------------------------------------------------- 1 | #ifndef FitAllanAcc_H 2 | #define FitAllanAcc_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace imu 9 | { 10 | 11 | class FitAllanAcc 12 | { 13 | class AllanSigmaError 14 | { 15 | public: 16 | AllanSigmaError( const double& _sigma2, const double& _tau ) 17 | : sigma2( _sigma2 ) 18 | , tau( _tau ) 19 | { 20 | } 21 | 22 | template< typename T > 23 | T calcLog10( T src ) const 24 | { 25 | return ( log( src ) ) / ( log( 10 ) ); 26 | } 27 | 28 | template< typename T > 29 | T calcSigma2( T _Q, T _N, T _B, T _K, T _R, T _tau ) const 30 | { 31 | // clang-format off 32 | return _Q * _Q / ( _tau * _tau ) 33 | + _N * _N / _tau 34 | + _B * _B 35 | + _K * _K * _tau 36 | + _R * _R * _tau * _tau; 37 | // clang-format on 38 | } 39 | 40 | template< typename T > 41 | bool operator( )( const T* const _paramt, T* residuals ) const 42 | { 43 | T _Q = T( _paramt[0] ); 44 | T _N = T( _paramt[1] ); 45 | T _B = T( _paramt[2] ); 46 | T _K = T( _paramt[3] ); 47 | T _R = T( _paramt[4] ); 48 | T _tau = T( tau ); 49 | 50 | T _sigma2 = calcSigma2( _Q, _N, _B, _K, _R, _tau ); 51 | T _dsigma2 = T( calcLog10( _sigma2 ) ) - T( calcLog10( sigma2 ) ); 52 | residuals[0] = _dsigma2; 53 | // std::cout << "_err " << T( sigma2 ) << " " << _sigma2 54 | // << std::endl; 55 | 56 | return true; 57 | } 58 | 59 | double sigma2; 60 | double tau; 61 | }; 62 | 63 | public: 64 | FitAllanAcc( std::vector< double > sigma2s, std::vector< double > taus, double _freq ); 65 | std::vector< double > calcSimDeviation( const std::vector< double > taus ) const; 66 | double getBiasInstability( ) const; 67 | double getWhiteNoise( ) const; 68 | 69 | private: 70 | std::vector< double > checkData( std::vector< double > sigma2s, std::vector< double > taus ); 71 | 72 | std::vector< double > initValue( std::vector< double > sigma2s, std::vector< double > taus ); 73 | double findMinNum( const std::vector< double > num ) const; 74 | int findMinIndex( std::vector< double > num ); 75 | double calcSigma2( double _Q, double _N, double _B, double _K, double _R, double _tau ) const; 76 | 77 | public: 78 | /** 79 | * @brief getQ 80 | * Quantization Noise 81 | * @unit: degree 82 | * @return 83 | */ 84 | double getQ( ) const; 85 | /** 86 | * @brief getN 87 | * Angle Random Walk 88 | * @unit: degree / sqrt( hour ) 89 | * @return 90 | */ 91 | double getN( ) const; 92 | /** 93 | * @brief getB 94 | * Bias Instability 95 | * @unit: degree / hour 96 | * @return 97 | */ 98 | double getB( ) const; 99 | /** 100 | * @brief getK 101 | * Rate Random Walk 102 | * @unit: degree / (hour*sqrt(hour)) 103 | * @return 104 | */ 105 | double getK( ) const; 106 | /** 107 | * @brief getR 108 | * Angle Rate Ramp 109 | * @unit: degree / (hour * hour) 110 | * @return 111 | */ 112 | double getR( ) const; 113 | 114 | double Q; 115 | double N; 116 | double B; 117 | double K; 118 | double R; 119 | 120 | private: 121 | std::vector< double > m_taus; 122 | double freq; 123 | }; 124 | } 125 | 126 | #endif // FitAllanAcc_H 127 | -------------------------------------------------------------------------------- /src/imu_utils/src/gyr_lib/allan_gyr.cpp: -------------------------------------------------------------------------------- 1 | #include "allan_gyr.h" 2 | 3 | imu::AllanGyr::AllanGyr( std::string name, int maxCluster ) 4 | : m_name( name ) 5 | , numData( 0 ) 6 | , numCluster( maxCluster ) 7 | { 8 | std::cout << m_name << " " 9 | << " num of Cluster " << numCluster << std::endl; 10 | } 11 | 12 | imu::AllanGyr::~AllanGyr( ) 13 | { 14 | m_rawData.clear( ); 15 | m_thetas.clear( ); 16 | mFactors.clear( ); 17 | } 18 | 19 | void 20 | imu::AllanGyr::pushRadPerSec( double data, double time ) 21 | { 22 | m_rawData.push_back( GyrData( data * 57.3 * 3600, time ) ); 23 | numData++; 24 | } 25 | 26 | void 27 | imu::AllanGyr::pushDegreePerSec( double data, double time ) 28 | { 29 | m_rawData.push_back( GyrData( data * 3600, time ) ); 30 | numData++; 31 | } 32 | 33 | void 34 | imu::AllanGyr::pushDegreePerHou( double data, double time ) 35 | { 36 | m_rawData.push_back( GyrData( data, time ) ); 37 | numData++; 38 | } 39 | 40 | void 41 | imu::AllanGyr::calc( ) 42 | { 43 | std::cout << m_name << " " 44 | << " numData " << numData << std::endl; 45 | if ( numData < 10000 ) 46 | std::cout << m_name << " " 47 | << " Too few number" << std::endl; 48 | 49 | double start_t = m_rawData.begin( )->t; 50 | double end_t = m_rawData[numData - 1].t; 51 | std::cout << m_name << " " 52 | << " start_t " << start_t << std::endl; 53 | std::cout << m_name << " " 54 | << " end_t " << end_t << std::endl; 55 | std::cout << m_name << " " 56 | << "dt " << std::endl // 57 | << "-------------" << ( end_t - start_t ) << " s" << std::endl 58 | << "-------------" << ( end_t - start_t ) / 60 << " min" << std::endl 59 | << "-------------" << ( end_t - start_t ) / 3600 << " h" << std::endl; 60 | 61 | if ( ( end_t - start_t ) / 60 < 10 ) 62 | std::cout << m_name << " " 63 | << " Too short time!!!!" << std::endl; 64 | 65 | m_freq = getAvgFreq( ); 66 | std::cout << m_name << " " 67 | << " freq " << m_freq << std::endl; 68 | 69 | double period = getAvgPeriod( ); 70 | std::cout << m_name << " " 71 | << " period " << period << std::endl; 72 | 73 | m_thetas = calcThetas( m_freq ); 74 | 75 | initStrides( ); 76 | 77 | mVariance = calcVariance( period ); 78 | } 79 | 80 | std::vector< double > 81 | imu::AllanGyr::getVariance( ) const 82 | { 83 | return mVariance; 84 | } 85 | 86 | std::vector< double > 87 | imu::AllanGyr::getDeviation( ) 88 | { 89 | double period = getAvgPeriod( ); 90 | 91 | std::vector< double > sigma2 = calcVariance( period ); 92 | std::vector< double > sigma; 93 | 94 | for ( auto& sig : sigma2 ) 95 | { 96 | sigma.push_back( sqrt( sig ) ); 97 | } 98 | return sigma; 99 | } 100 | 101 | std::vector< double > 102 | imu::AllanGyr::getTimes( ) 103 | { 104 | double period = getAvgPeriod( ); 105 | std::vector< double > time( numFactors, 0.0 ); 106 | for ( int i = 0; i < numFactors; i++ ) 107 | { 108 | int factor = mFactors[i]; 109 | // double clusterPeriod2 = ( period * factor ) * ( period * factor ); 110 | time[i] = period * factor; 111 | } 112 | return time; 113 | } 114 | 115 | std::vector< int > 116 | imu::AllanGyr::getFactors( ) const 117 | { 118 | return mFactors; 119 | } 120 | 121 | std::vector< double > 122 | imu::AllanGyr::calcVariance( double period ) 123 | { 124 | std::vector< double > sigma2( numFactors, 0.0 ); 125 | 126 | for ( int i = 0; i < numFactors; i++ ) 127 | { 128 | int factor = mFactors[i]; 129 | double clusterPeriod2 = ( period * factor ) * ( period * factor ); 130 | double divided = 2 * clusterPeriod2 * ( numData - 2 * factor ); 131 | int max = numData - 2 * factor; 132 | 133 | for ( int k = 0; k < max; k++ ) 134 | { 135 | double temp = ( m_thetas[k + 2 * factor] - 2 * m_thetas[k + factor] + m_thetas[k] ); 136 | sigma2[i] += ( temp * temp ); 137 | } 138 | 139 | // std::cout << "sigma2 " << sigma2[i] << std::endl; 140 | sigma2[i] = sigma2[i] / divided; 141 | } 142 | 143 | // for ( int index = 0; index < mVariance.size( ); ++index ) 144 | // { 145 | // std::cout << sigma2[index] << std::endl; 146 | // } 147 | return sigma2; 148 | } 149 | 150 | std::vector< double > 151 | imu::AllanGyr::calcThetas( const double freq ) 152 | { 153 | std::vector< double > thetas; 154 | 155 | double sum = 0; 156 | for ( auto& gyro : m_rawData ) 157 | { 158 | sum += gyro.w; 159 | thetas.push_back( sum / freq ); 160 | } 161 | return thetas; 162 | } 163 | 164 | void 165 | imu::AllanGyr::initStrides( ) 166 | { 167 | 168 | int mode = numData / 2; 169 | unsigned int maxStride = 1; 170 | int shft = 0; 171 | while ( mode ) 172 | { 173 | mode = mode >> 1; 174 | maxStride = 1 << shft; 175 | shft++; 176 | } 177 | // std::cout << m_name << " " 178 | // << " mode: " << mode << std::endl; 179 | // std::cout << m_name << " " 180 | // << " shft: " << shft << std::endl; 181 | // std::cout << m_name << " " 182 | // << " maxStride: " << maxStride << std::endl; 183 | 184 | std::vector< double > avgFactors = getLogSpace( 0, log10( maxStride ) ); 185 | // std::cout << m_name << " " 186 | // << " avgFactors " << avgFactors.size( ) << std::endl; 187 | 188 | for ( int i = 0; i < numCluster; i++ ) 189 | { 190 | // std::cout << m_name << " " 191 | // << " avgFactors " << i << " " << avgFactors[i] << std::endl; 192 | avgFactors[i] = ceil( avgFactors[i] ); 193 | } 194 | 195 | std::vector< int > factors( numCluster, 0 ); 196 | 197 | numFactors = 1; 198 | factors[0] = ( int )avgFactors[0]; 199 | 200 | for ( int i = 1; i < numCluster; i++ ) 201 | { 202 | // std::cout << m_name << " " 203 | // << " avgFactors " << i << " " << avgFactors[i] << std::endl; 204 | if ( avgFactors[i] != avgFactors[i - 1] ) 205 | { 206 | factors[numFactors] = ( int )avgFactors[i]; 207 | numFactors++; 208 | } 209 | } 210 | 211 | mFactors = factors; 212 | 213 | // std::cout << m_name << " " 214 | // << " numFactors " << numFactors << std::endl; 215 | } 216 | 217 | std::vector< double > 218 | imu::AllanGyr::getLogSpace( float a, float b ) 219 | { 220 | std::vector< double > logSpace; 221 | 222 | double start = pow( 10, a ); 223 | double end = pow( 10, b ); 224 | double progression = pow( end / start, ( float )1 / ( numCluster - 1 ) ); 225 | 226 | logSpace.push_back( start ); 227 | for ( int i = 1; i < numCluster; i++ ) 228 | { 229 | logSpace.push_back( logSpace[i - 1] * progression ); 230 | } 231 | // std::cout << m_name << " " 232 | // << "size logSpace " << logSpace.size( ) << std::endl; 233 | return logSpace; 234 | } 235 | 236 | double 237 | imu::AllanGyr::getAvgDt( ) 238 | { 239 | double sum_dt = 0.0; 240 | double start_t = m_rawData[0].t; 241 | bool first = true; 242 | for ( auto& gyro : m_rawData ) 243 | { 244 | if ( !first ) 245 | sum_dt += ( gyro.t - start_t ); 246 | start_t = gyro.t; 247 | first = false; 248 | } 249 | return sum_dt / ( numData - 1 ); 250 | } 251 | 252 | double 253 | imu::AllanGyr::getAvgValue( ) 254 | { 255 | double sum = 0.0; 256 | int num = 0; 257 | for ( auto& gyro : m_rawData ) 258 | { 259 | sum += gyro.w; 260 | ++num; 261 | } 262 | return sum / num; 263 | } 264 | 265 | double 266 | imu::AllanGyr::getFreq( ) const 267 | { 268 | return m_freq; 269 | } 270 | -------------------------------------------------------------------------------- /src/imu_utils/src/gyr_lib/allan_gyr.h: -------------------------------------------------------------------------------- 1 | #ifndef ALLAN_H 2 | #define ALLAN_H 3 | 4 | #include "../type.h" 5 | #include 6 | #include 7 | #include 8 | 9 | namespace imu 10 | { 11 | 12 | class AllanGyr 13 | { 14 | public: 15 | AllanGyr( std::string name, int maxCluster = 10000 ); 16 | ~AllanGyr( ); 17 | void pushRadPerSec( double data, double time ); 18 | void pushDegreePerSec( double data, double time ); 19 | void pushDegreePerHou( double data, double time ); 20 | void calc( ); 21 | 22 | std::vector< double > getVariance( ) const; 23 | std::vector< double > getDeviation( ); 24 | std::vector< double > getTimes( ); 25 | std::vector< int > getFactors( ) const; 26 | double getAvgValue( ); 27 | double getFreq( ) const; 28 | 29 | private: 30 | std::vector< double > calcVariance( double period ); 31 | 32 | std::vector< double > calcThetas( const double freq ); 33 | void initStrides( ); 34 | std::vector< double > getLogSpace( float a, float b ); 35 | double getAvgFreq( ) { return 1.0 / getAvgDt( ); } 36 | double getAvgDt( ); 37 | double getAvgPeriod( ) { return getAvgDt( ); } 38 | int getFactorsNum( ) { return numFactors; } 39 | 40 | std::string m_name; 41 | double m_freq; 42 | int numData; 43 | std::vector< GyrData > m_rawData; 44 | std::vector< double > m_thetas; 45 | int numCluster; 46 | int numFactors; 47 | std::vector< int > mFactors; 48 | 49 | std::vector< double > mVariance; 50 | }; 51 | } 52 | 53 | #endif // ALLAN_H 54 | -------------------------------------------------------------------------------- /src/imu_utils/src/gyr_lib/fitallan_gyr.cpp: -------------------------------------------------------------------------------- 1 | #include "fitallan_gyr.h" 2 | 3 | using namespace imu; 4 | 5 | FitAllanGyr::FitAllanGyr( std::vector< double > sigma2s, std::vector< double > taus, double _freq ) 6 | : Q( 0.0 ) 7 | , N( 0.0 ) 8 | , B( 0.0 ) 9 | , K( 0.0 ) 10 | , R( 0.0 ) 11 | , freq( _freq ) 12 | { 13 | if ( sigma2s.size( ) != taus.size( ) ) 14 | std::cerr << "Error of point size" << std::endl; 15 | 16 | m_taus = taus; 17 | 18 | std::vector< double > init = initValue( sigma2s, taus ); 19 | 20 | int num_samples = sigma2s.size( ); 21 | // double param[] = { Q, N, B, K, R }; 22 | double param[] = { init[0], init[1], init[2], init[3], init[4] }; 23 | 24 | ceres::Problem problem; 25 | 26 | for ( int i = 0; i < num_samples; ++i ) 27 | { 28 | // std::cout << "sigma " << i << " " << taus[i] << " " << sigma2s[i] << std::endl; 29 | 30 | ceres::CostFunction* f = new ceres::AutoDiffCostFunction< AllanSigmaError, 1, 5 >( 31 | new AllanSigmaError( sigma2s[i], taus[i] ) ); 32 | 33 | problem.AddResidualBlock( f, NULL /* squared loss */, param ); 34 | } 35 | ceres::Solver::Options options; 36 | options.minimizer_progress_to_stdout = true; 37 | options.logging_type = ceres::SILENT; 38 | options.trust_region_strategy_type = ceres::DOGLEG; 39 | // options.max_num_iterations = 5; 40 | 41 | ceres::Solver::Summary summary; 42 | ceres::Solve( options, &problem, &summary ); 43 | // std::cout << summary.FullReport( ) << "\n"; 44 | // std::cout << "num_parameters " << summary.num_parameters << std::endl; 45 | 46 | Q = param[0]; 47 | N = param[1]; 48 | B = param[2]; 49 | K = param[3]; 50 | R = param[4]; 51 | 52 | // std::cout << "Q " << Q // 53 | // << " " << N // 54 | // << " " << B // 55 | // << " " << K // 56 | // << " " << R << std::endl; 57 | 58 | std::cout << " Bias Instability " << getB( ) / ( 57.3 * 3600 ) << " rad/s" << std::endl; 59 | std::cout << " Bias Instability " << getBiasInstability( ) << " rad/s, at " 60 | << taus[findMinIndex( calcSimDeviation( taus ) )] << " s" << std::endl; 61 | 62 | std::cout << " White Noise " << sqrt( freq ) * getN( ) * 60 / 57.3 << " rad/s" << std::endl; 63 | std::cout << " White Noise " << getWhiteNoise( ) << " rad/s" << std::endl; 64 | } 65 | 66 | std::vector< double > 67 | FitAllanGyr::initValue( std::vector< double > sigma2s, std::vector< double > taus ) 68 | { 69 | if ( sigma2s.size( ) != taus.size( ) ) 70 | std::cout << " Error with data size!!!" << std::endl; 71 | 72 | Eigen::MatrixXd Y( sigma2s.size( ), 1 ); 73 | 74 | for ( unsigned int index = 0; index < sigma2s.size( ); ++index ) 75 | { 76 | Y( index, 0 ) = sqrt( sigma2s[index] ); 77 | } 78 | // std::cout << "Y " << Y << std::endl; 79 | 80 | int m_order = 2; 81 | 82 | Eigen::MatrixXd B( 2 * m_order + 1, 1 ); 83 | B.setZero( ); 84 | 85 | Eigen::MatrixXd F( taus.size( ), 2 * m_order + 1 ); 86 | F.setZero( ); 87 | 88 | for ( unsigned int index = 0; index < taus.size( ); ++index ) 89 | for ( int order_index = 0; order_index < 2 * m_order + 1; ++order_index ) 90 | { 91 | int kk = order_index - m_order; 92 | F( index, order_index ) = pow( sqrt( taus[index] ), kk ); 93 | } 94 | // std::cout << "F " << F << std::endl; 95 | 96 | Eigen::MatrixXd A = F.transpose( ) * F; 97 | B = F.transpose( ) * Y; 98 | // std::cout << "B " << B << std::endl; 99 | // std::cout << "A " << A << std::endl; 100 | 101 | Eigen::MatrixXd C = A.inverse( ) * B; 102 | std::cout << "C " << C.transpose( ) << std::endl; 103 | 104 | std::vector< double > init; 105 | for ( int index = 0; index < 2 * m_order + 1; ++index ) 106 | init.push_back( std::abs( C( index, 0 ) ) ); 107 | 108 | return init; 109 | } 110 | 111 | std::vector< double > 112 | FitAllanGyr::calcSimDeviation( const std::vector< double > taus ) const 113 | { 114 | std::vector< double > des; 115 | for ( auto& tau : taus ) 116 | des.push_back( sqrt( calcSigma2( Q, N, B, K, R, tau ) ) ); 117 | return des; 118 | } 119 | 120 | double 121 | FitAllanGyr::getBiasInstability( ) const 122 | { 123 | return findMinNum( calcSimDeviation( m_taus ) ) / ( 57.3 * 3600 ); 124 | } 125 | 126 | double 127 | FitAllanGyr::getWhiteNoise( ) const 128 | { 129 | return sqrt( freq ) * sqrt( calcSigma2( Q, N, B, K, R, 1 ) ) / ( 57.3 * 3600 ); 130 | } 131 | 132 | double 133 | FitAllanGyr::findMinNum( const std::vector< double > num ) const 134 | { 135 | double min = 1000.0; 136 | for ( unsigned int index = 0; index < num.size( ); ++index ) 137 | min = min < num[index] ? min : num[index]; 138 | return min; 139 | } 140 | 141 | int 142 | FitAllanGyr::findMinIndex( std::vector< double > num ) 143 | { 144 | double min = 1000.0; 145 | int min_index = 0; 146 | for ( unsigned int index = 0; index < num.size( ); ++index ) 147 | { 148 | min_index = min < num[index] ? min_index : index; 149 | min = min < num[index] ? min : num[index]; 150 | } 151 | return min_index; 152 | } 153 | 154 | double 155 | FitAllanGyr::calcSigma2( double _Q, double _N, double _B, double _K, double _R, double _tau ) const 156 | { 157 | // clang-format off 158 | return _Q * _Q / ( _tau * _tau ) 159 | + _N * _N / _tau 160 | + _B * _B 161 | + _K * _K * _tau 162 | + _R * _R * _tau * _tau; 163 | // clang-format on 164 | } 165 | 166 | double 167 | FitAllanGyr::getN( ) const 168 | { 169 | return sqrt( N * N ) / 60.0; 170 | } 171 | 172 | double 173 | FitAllanGyr::getB( ) const 174 | { 175 | return sqrt( B * B ) / 0.6642824703; 176 | } 177 | 178 | double 179 | FitAllanGyr::getK( ) const 180 | { 181 | return 60.0 * sqrt( 3.0 * K * K ); 182 | } 183 | 184 | double 185 | FitAllanGyr::getR( ) const 186 | { 187 | return 3600.0 * sqrt( 2.0 * R * R ); 188 | } 189 | 190 | double 191 | FitAllanGyr::getQ( ) const 192 | { 193 | return sqrt( Q * Q ) / ( 3600.0 * sqrt( 3.0 ) ); 194 | } 195 | -------------------------------------------------------------------------------- /src/imu_utils/src/gyr_lib/fitallan_gyr.h: -------------------------------------------------------------------------------- 1 | #ifndef FitAllanGyr_H 2 | #define FitAllanGyr_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace imu 9 | { 10 | 11 | class FitAllanGyr 12 | { 13 | class AllanSigmaError 14 | { 15 | public: 16 | AllanSigmaError( const double& _sigma2, const double& _tau ) 17 | : sigma2( _sigma2 ) 18 | , tau( _tau ) 19 | { 20 | } 21 | 22 | template< typename T > 23 | T calcLog10( T src ) const 24 | { 25 | return ( log( src ) ) / ( log( 10 ) ); 26 | } 27 | 28 | template< typename T > 29 | T calcSigma2( T _Q, T _N, T _B, T _K, T _R, T _tau ) const 30 | { 31 | // clang-format off 32 | return _Q * _Q / ( _tau * _tau ) 33 | + _N * _N / _tau 34 | + _B * _B 35 | + _K * _K * _tau 36 | + _R * _R * _tau * _tau; 37 | // clang-format on 38 | } 39 | 40 | template< typename T > 41 | bool operator( )( const T* const _paramt, T* residuals ) const 42 | { 43 | T _Q = T( _paramt[0] ); 44 | T _N = T( _paramt[1] ); 45 | T _B = T( _paramt[2] ); 46 | T _K = T( _paramt[3] ); 47 | T _R = T( _paramt[4] ); 48 | T _tau = T( tau ); 49 | 50 | T _sigma2 = calcSigma2( _Q, _N, _B, _K, _R, _tau ); 51 | T _dsigma2 = T( calcLog10( _sigma2 ) ) - T( calcLog10( sigma2 ) ); 52 | residuals[0] = _dsigma2; 53 | // std::cout << "_err " << T( sigma2 ) << " " << _sigma2 << std::endl; 54 | 55 | return true; 56 | } 57 | 58 | double sigma2; 59 | double tau; 60 | }; 61 | 62 | public: 63 | FitAllanGyr( std::vector< double > sigma2s, std::vector< double > taus, double _freq ); 64 | std::vector< double > calcSimDeviation( const std::vector< double > taus ) const; 65 | double getBiasInstability( ) const; 66 | double getWhiteNoise( ) const; 67 | 68 | private: 69 | std::vector< double > initValue( std::vector< double > sigma2s, std::vector< double > taus ); 70 | double findMinNum( const std::vector< double > num ) const; 71 | int findMinIndex( std::vector< double > num ); 72 | double calcSigma2( double _Q, double _N, double _B, double _K, double _R, double _tau ) const; 73 | 74 | public: 75 | /** 76 | * @brief getQ 77 | * Quantization Noise 78 | * @unit: degree 79 | * @return 80 | */ 81 | double getQ( ) const; 82 | /** 83 | * @brief getN 84 | * Angle Random Walk 85 | * @unit: degree / sqrt( hour ) 86 | * @return 87 | */ 88 | double getN( ) const; 89 | /** 90 | * @brief getB 91 | * Bias Instability 92 | * @unit: degree / hour 93 | * @return 94 | */ 95 | double getB( ) const; 96 | /** 97 | * @brief getK 98 | * Rate Random Walk 99 | * @unit: degree / (hour*sqrt(hour)) 100 | * @return 101 | */ 102 | double getK( ) const; 103 | /** 104 | * @brief getR 105 | * Angle Rate Ramp 106 | * @unit: degree / (hour * hour) 107 | * @return 108 | */ 109 | double getR( ) const; 110 | 111 | double Q; 112 | double N; 113 | double B; 114 | double K; 115 | double R; 116 | 117 | private: 118 | std::vector< double > m_taus; 119 | double freq; 120 | }; 121 | } 122 | #endif // FitAllanGyr_H 123 | -------------------------------------------------------------------------------- /src/imu_utils/src/type.h: -------------------------------------------------------------------------------- 1 | #ifndef TYPE_H 2 | #define TYPE_H 3 | 4 | class Data 5 | { 6 | public: 7 | Data( ) 8 | : v( 0.0 ) 9 | , t( 0.0 ) 10 | { 11 | } 12 | Data( double data, double time ) 13 | : v( data ) 14 | , t( time ) 15 | { 16 | } 17 | 18 | double v; 19 | double t; 20 | }; 21 | 22 | class AccData 23 | { 24 | public: 25 | AccData( ) 26 | : a( 0.0 ) 27 | , t( 0.0 ) 28 | { 29 | } 30 | AccData( double data, double time ) 31 | : a( data ) 32 | , t( time ) 33 | { 34 | } 35 | 36 | double a; 37 | double t; 38 | }; 39 | 40 | class GyrData 41 | { 42 | public: 43 | GyrData( ) 44 | : w( 0.0 ) 45 | , t( 0.0 ) 46 | { 47 | } 48 | GyrData( double data, double time ) 49 | : w( data ) 50 | , t( time ) 51 | { 52 | } 53 | 54 | double w; 55 | double t; 56 | }; 57 | 58 | #endif // TYPE_H 59 | -------------------------------------------------------------------------------- /src/imu_utils/src/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILS_H 2 | #define UTILS_H 3 | 4 | #include 5 | 6 | #define M_PI_CON 3.141592653589 7 | 8 | namespace utils 9 | { 10 | template< class T > 11 | T 12 | avg( const std::vector< T > datas ) 13 | { 14 | T sum_data = T( 0 ); 15 | for ( auto data : datas ) 16 | sum_data += data; 17 | return sum_data / datas.size( ); 18 | } 19 | } 20 | 21 | #endif // UTILS_H 22 | -------------------------------------------------------------------------------- /src/vio_data_simulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vio_data_simulation) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | rospy 9 | std_msgs 10 | rosbag 11 | tf 12 | nav_msgs 13 | sensor_msgs 14 | geometry_msgs 15 | ) 16 | 17 | find_package(Boost REQUIRED COMPONENTS system) 18 | find_package(Eigen3 REQUIRED) 19 | 20 | 21 | catkin_package( 22 | CATKIN_DEPENDS roscpp rospy std_msgs 23 | 24 | ) 25 | 26 | include_directories( 27 | #include 28 | ${catkin_INCLUDE_DIRS} 29 | ${EIGEN_INCLUDE_DIRS} 30 | ) 31 | 32 | add_executable(${PROJECT_NAME}_node src/gener_alldata.cpp src/param.h src/param.cpp src/utilities.h src/utilities.cpp src/imu.h src/imu.cpp) 33 | target_link_libraries(${PROJECT_NAME}_node 34 | ${catkin_LIBRARIES} 35 | ${EIGEN_LIBRARIES} 36 | ) 37 | 38 | 39 | -------------------------------------------------------------------------------- /src/vio_data_simulation/README.md: -------------------------------------------------------------------------------- 1 | # imusim with ROS bag 2 | imu 和 cam 数据仿真, 用于vio算法测试，代码有任何问题都欢迎交流 heyijia_2013@163.com，另可内推旷视科技SLAM组。 3 | 4 | ![demo pic](https://github.com/HeYijia/vio_data_simulation/blob/master/bin/demo.png?raw=true) 5 | 6 | ## 坐标系 7 | **B**ody frame: imu坐标系 8 | 9 | **C**am frame: 相机坐标系 10 | 11 | **W**orld　frame: imu坐标系的第一帧位置 12 | 13 | **N**avigation frame: NED(东北天) or　ENU（北东地）,本代码采用的是ENU,重力向量在该坐标系下为$(0,0,-9.81)$ 14 | 15 | 目前，imu的z轴向上，xy平面内做椭圆运动，z轴做正弦运动，x轴沿着圆周向外。外参数Tbc将相机坐标旋转，使得相机朝向特征点。 16 | 17 | ## 代码编译 18 | 19 | 1. mkdir vio_sim_ws/src 20 | 2. cd src 21 | 3. git clone https://github.com/HeYijia/vio_data_simulation 22 | 4. git checkout ros_version 23 | 5. catkin_make 24 | 25 | ## 代码结构 26 | 27 | main/gener_alldata.cpp : 用于生成 imu 数据， **并写进 ROS bag 文件** 28 | 29 | src/paramc.h：imu噪声参数，imu频率，相机内参数等等 30 | 31 | ## 数据存储的格式 32 | ### 特征点 33 | x，y，z，1，u，v 34 | 35 | 每个特征出现在文件里的顺序，就是他们独立的id，可用来检索特征匹配 36 | 37 | ### imu data 38 | timestamp (1)，imu quaternion(4)，imu position(3)，imu gyro(3)，imu acc(3) 39 | 40 | ### cam data 41 | timestamp (1)，cam quaternion(4)，cam position(3)，imu gyro(3)，imu acc(3) 42 | 43 | 注意，由于imu和cam的存储采用的是同一个函数，所以cam也会存储一些gyro,acc这些数据，但是没用，是多余存储的。 44 | -------------------------------------------------------------------------------- /src/vio_data_simulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vio_data_simulation 4 | 0.0.0 5 | The vio_data_simulation package 6 | 7 | 8 | 9 | 10 | liudong 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 | 64 | catkin 65 | roscpp 66 | rospy 67 | std_msgs 68 | roscpp 69 | rospy 70 | std_msgs 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /src/vio_data_simulation/rviz/vio_simulation.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /Path1 11 | - /Path2 12 | - /Path3 13 | - /Odometry1 14 | - /Odometry1/Shape1 15 | - /Odometry1/Covariance1/Position1 16 | - /TF1/Frames1 17 | - /TF1/Tree1 18 | - /Imu1/Box properties1 19 | - /Imu1/Axes properties1 20 | - /Imu1/Acceleration properties1 21 | Splitter Ratio: 0.564920247 22 | Tree Height: 775 23 | - Class: rviz/Selection 24 | Name: Selection 25 | - Class: rviz/Tool Properties 26 | Expanded: 27 | - /2D Pose Estimate1 28 | - /2D Nav Goal1 29 | - /Publish Point1 30 | Name: Tool Properties 31 | Splitter Ratio: 0.588679016 32 | - Class: rviz/Views 33 | Expanded: 34 | - /Current View1 35 | Name: Views 36 | Splitter Ratio: 0.5 37 | - Class: rviz/Time 38 | Experimental: false 39 | Name: Time 40 | SyncMode: 0 41 | SyncSource: "" 42 | Visualization Manager: 43 | Class: "" 44 | Displays: 45 | - Alpha: 0.200000003 46 | Cell Size: 1 47 | Class: rviz/Grid 48 | Color: 160; 160; 164 49 | Enabled: true 50 | Line Style: 51 | Line Width: 0.0299999993 52 | Value: Lines 53 | Name: Grid 54 | Normal Cell Count: 0 55 | Offset: 56 | X: 0 57 | Y: 0 58 | Z: 0 59 | Plane: XY 60 | Plane Cell Count: 50 61 | Reference Frame: 62 | Value: true 63 | - Alpha: 1 64 | Buffer Length: 1 65 | Class: rviz/Path 66 | Color: 25; 255; 0 67 | Enabled: true 68 | Head Diameter: 0.300000012 69 | Head Length: 0.200000003 70 | Length: 0.300000012 71 | Line Style: Lines 72 | Line Width: 0.0299999993 73 | Name: Path 74 | Offset: 75 | X: 0 76 | Y: 0 77 | Z: 0 78 | Pose Color: 255; 85; 255 79 | Pose Style: None 80 | Radius: 0.0299999993 81 | Shaft Diameter: 0.100000001 82 | Shaft Length: 0.100000001 83 | Topic: /real_path 84 | Unreliable: false 85 | Value: true 86 | - Alpha: 1 87 | Buffer Length: 1 88 | Class: rviz/Path 89 | Color: 255; 0; 0 90 | Enabled: true 91 | Head Diameter: 0.300000012 92 | Head Length: 0.200000003 93 | Length: 0.300000012 94 | Line Style: Lines 95 | Line Width: 0.0299999993 96 | Name: Path 97 | Offset: 98 | X: 0 99 | Y: 0 100 | Z: 0 101 | Pose Color: 255; 85; 255 102 | Pose Style: None 103 | Radius: 0.0299999993 104 | Shaft Diameter: 0.100000001 105 | Shaft Length: 0.100000001 106 | Topic: /euler_path 107 | Unreliable: false 108 | Value: true 109 | - Alpha: 1 110 | Buffer Length: 1 111 | Class: rviz/Path 112 | Color: 255; 255; 255 113 | Enabled: true 114 | Head Diameter: 0.300000012 115 | Head Length: 0.200000003 116 | Length: 0.300000012 117 | Line Style: Lines 118 | Line Width: 0.0299999993 119 | Name: Path 120 | Offset: 121 | X: 0 122 | Y: 0 123 | Z: 0 124 | Pose Color: 255; 85; 255 125 | Pose Style: None 126 | Radius: 0.0299999993 127 | Shaft Diameter: 0.100000001 128 | Shaft Length: 0.100000001 129 | Topic: /mid_path 130 | Unreliable: false 131 | Value: true 132 | - Class: rviz/MarkerArray 133 | Enabled: false 134 | Marker Topic: /beforePoseGraph 135 | Name: MarkerArray 136 | Namespaces: 137 | {} 138 | Queue Size: 100 139 | Value: false 140 | - Class: rviz/MarkerArray 141 | Enabled: false 142 | Marker Topic: /afterPoseGraph 143 | Name: MarkerArray 144 | Namespaces: 145 | {} 146 | Queue Size: 100 147 | Value: false 148 | - Alpha: 0.699999988 149 | Class: rviz/Map 150 | Color Scheme: map 151 | Draw Behind: false 152 | Enabled: false 153 | Name: Map 154 | Topic: /laser_map 155 | Unreliable: false 156 | Use Timestamp: false 157 | Value: false 158 | - Angle Tolerance: 0.100000001 159 | Class: rviz/Odometry 160 | Covariance: 161 | Orientation: 162 | Alpha: 0.5 163 | Color: 255; 255; 127 164 | Color Style: Unique 165 | Frame: Local 166 | Offset: 1 167 | Scale: 1 168 | Value: true 169 | Position: 170 | Alpha: 0.300000012 171 | Color: 204; 51; 204 172 | Scale: 1 173 | Value: true 174 | Value: true 175 | Enabled: true 176 | Keep: 1 177 | Name: Odometry 178 | Position Tolerance: 0.100000001 179 | Shape: 180 | Alpha: 1 181 | Axes Length: 1 182 | Axes Radius: 0.100000001 183 | Color: 255; 25; 0 184 | Head Length: 0.300000012 185 | Head Radius: 0.100000001 186 | Shaft Length: 1 187 | Shaft Radius: 0.0500000007 188 | Value: Arrow 189 | Topic: /real_odom 190 | Unreliable: false 191 | Value: true 192 | - Alpha: 1 193 | Autocompute Intensity Bounds: true 194 | Autocompute Value Bounds: 195 | Max Value: 10 196 | Min Value: -10 197 | Value: true 198 | Axis: Z 199 | Channel Name: intensity 200 | Class: rviz/LaserScan 201 | Color: 255; 255; 255 202 | Color Transformer: Intensity 203 | Decay Time: 0 204 | Enabled: false 205 | Invert Rainbow: false 206 | Max Color: 255; 255; 255 207 | Max Intensity: 242 208 | Min Color: 0; 0; 0 209 | Min Intensity: 3 210 | Name: LaserScan 211 | Position Transformer: XYZ 212 | Queue Size: 10 213 | Selectable: true 214 | Size (Pixels): 3 215 | Size (m): 0.0500000007 216 | Style: Flat Squares 217 | Topic: /sick_scan 218 | Unreliable: false 219 | Use Fixed Frame: true 220 | Use rainbow: true 221 | Value: false 222 | - Alpha: 1 223 | Buffer Length: 1 224 | Class: rviz/Path 225 | Color: 25; 255; 0 226 | Enabled: false 227 | Head Diameter: 0.300000012 228 | Head Length: 0.200000003 229 | Length: 0.300000012 230 | Line Style: Lines 231 | Line Width: 0.0299999993 232 | Name: Path 233 | Offset: 234 | X: 0 235 | Y: 0 236 | Z: 0 237 | Pose Color: 255; 85; 255 238 | Pose Style: None 239 | Radius: 0.0299999993 240 | Shaft Diameter: 0.100000001 241 | Shaft Length: 0.100000001 242 | Topic: "" 243 | Unreliable: false 244 | Value: false 245 | - Class: rviz/TF 246 | Enabled: true 247 | Frame Timeout: 15 248 | Frames: 249 | All Enabled: true 250 | imu_link: 251 | Value: true 252 | vio_odom_link: 253 | Value: true 254 | Marker Scale: 1 255 | Name: TF 256 | Show Arrows: true 257 | Show Axes: true 258 | Show Names: true 259 | Tree: 260 | vio_odom_link: 261 | imu_link: 262 | {} 263 | Update Interval: 0 264 | Value: true 265 | - Alpha: 1 266 | Autocompute Intensity Bounds: true 267 | Autocompute Value Bounds: 268 | Max Value: 10 269 | Min Value: -10 270 | Value: true 271 | Axis: Z 272 | Channel Name: intensity 273 | Class: rviz/PointCloud 274 | Color: 255; 255; 255 275 | Color Transformer: "" 276 | Decay Time: 0 277 | Enabled: false 278 | Invert Rainbow: false 279 | Max Color: 255; 255; 255 280 | Max Intensity: 4096 281 | Min Color: 0; 0; 0 282 | Min Intensity: 0 283 | Name: PointCloud 284 | Position Transformer: "" 285 | Queue Size: 10 286 | Selectable: true 287 | Size (Pixels): 3 288 | Size (m): 0.00999999978 289 | Style: Flat Squares 290 | Topic: /laser_cloud 291 | Unreliable: false 292 | Use Fixed Frame: true 293 | Use rainbow: true 294 | Value: false 295 | - Acceleration properties: 296 | Acc. vector alpha: 1 297 | Acc. vector color: 255; 0; 0 298 | Acc. vector scale: 1 299 | Derotate acceleration: true 300 | Enable acceleration: true 301 | Axes properties: 302 | Axes scale: 2 303 | Enable axes: true 304 | Box properties: 305 | Box alpha: 1 306 | Box color: 255; 0; 0 307 | Enable box: true 308 | x_scale: 1 309 | y_scale: 1 310 | z_scale: 1 311 | Class: rviz_imu_plugin/Imu 312 | Enabled: true 313 | Name: Imu 314 | Topic: /imu 315 | Unreliable: false 316 | Value: true 317 | fixed_frame_orientation: true 318 | Enabled: true 319 | Global Options: 320 | Background Color: 48; 48; 48 321 | Default Light: true 322 | Fixed Frame: vio_odom_link 323 | Frame Rate: 30 324 | Name: root 325 | Tools: 326 | - Class: rviz/Interact 327 | Hide Inactive Objects: true 328 | - Class: rviz/MoveCamera 329 | - Class: rviz/Select 330 | - Class: rviz/FocusCamera 331 | - Class: rviz/Measure 332 | - Class: rviz/SetInitialPose 333 | Topic: /initialpose 334 | - Class: rviz/SetGoal 335 | Topic: /move_base_simple/goal 336 | - Class: rviz/PublishPoint 337 | Single click: true 338 | Topic: /clicked_point 339 | Value: true 340 | Views: 341 | Current: 342 | Class: rviz/ThirdPersonFollower 343 | Distance: 50.4945717 344 | Enable Stereo Rendering: 345 | Stereo Eye Separation: 0.0599999987 346 | Stereo Focal Distance: 1 347 | Swap Stereo Eyes: false 348 | Value: false 349 | Focal Point: 350 | X: 0 351 | Y: 0 352 | Z: 0 353 | Focal Shape Fixed Size: true 354 | Focal Shape Size: 0.0500000007 355 | Invert Z Axis: false 356 | Name: Current View 357 | Near Clip Distance: 0.00999999978 358 | Pitch: 0.310397863 359 | Target Frame: 360 | Value: ThirdPersonFollower (rviz) 361 | Yaw: 2.54857326 362 | Saved: ~ 363 | Window Geometry: 364 | Displays: 365 | collapsed: false 366 | Height: 1056 367 | Hide Left Dock: false 368 | Hide Right Dock: false 369 | QMainWindow State: 000000ff00000000fd0000000400000000000001c600000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000045e0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 370 | Selection: 371 | collapsed: false 372 | Time: 373 | collapsed: false 374 | Tool Properties: 375 | collapsed: false 376 | Views: 377 | collapsed: false 378 | Width: 1855 379 | X: 65 380 | Y: 24 381 | -------------------------------------------------------------------------------- /src/vio_data_simulation/src/imu.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hyj on 18-1-19. 3 | // 4 | 5 | #include "imu.h" 6 | #include "utilities.h" 7 | 8 | // euler2Rotation: body frame to interitail frame 9 | Eigen::Matrix3d euler2Rotation( Eigen::Vector3d eulerAngles) 10 | { 11 | double roll = eulerAngles(0); 12 | double pitch = eulerAngles(1); 13 | double yaw = eulerAngles(2); 14 | 15 | double cr = cos(roll); double sr = sin(roll); 16 | double cp = cos(pitch); double sp = sin(pitch); 17 | double cy = cos(yaw); double sy = sin(yaw); 18 | 19 | Eigen::Matrix3d RIb; 20 | RIb<< cy*cp , cy*sp*sr - sy*cr, sy*sr + cy* cr*sp, 21 | sy*cp, cy *cr + sy*sr*sp, sp*sy*cr - cy*sr, 22 | -sp, cp*sr, cp*cr; 23 | return RIb; 24 | } 25 | 26 | Eigen::Matrix3d eulerRates2bodyRates(Eigen::Vector3d eulerAngles) 27 | { 28 | double roll = eulerAngles(0); 29 | double pitch = eulerAngles(1); 30 | 31 | double cr = cos(roll); double sr = sin(roll); 32 | double cp = cos(pitch); double sp = sin(pitch); 33 | 34 | Eigen::Matrix3d R; 35 | R<< 1, 0, -sp, 36 | 0, cr, sr*cp, 37 | 0, -sr, cr*cp; 38 | 39 | return R; 40 | } 41 | 42 | 43 | IMU::IMU(Param p): param_(p) 44 | { 45 | gyro_bias_ = Eigen::Vector3d::Zero(); 46 | acc_bias_ = Eigen::Vector3d::Zero(); 47 | 48 | MotionData data = MotionModel(0.001); 49 | init_velocity_ = data.imu_velocity; 50 | init_twb_ = data.twb; 51 | init_Rwb_ = data.Rwb; 52 | 53 | } 54 | 55 | void IMU::addIMUnoise(MotionData& data) 56 | { 57 | std::random_device rd; 58 | std::default_random_engine generator_(rd()); 59 | std::normal_distribution noise(0.0, 1.0); 60 | 61 | Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_)); 62 | Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity(); 63 | data.imu_gyro = data.imu_gyro + gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) + gyro_bias_; 64 | 65 | Eigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_)); 66 | Eigen::Matrix3d acc_sqrt_cov = param_.acc_noise_sigma * Eigen::Matrix3d::Identity(); 67 | data.imu_acc = data.imu_acc + acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) + acc_bias_; 68 | 69 | // gyro_bias update 70 | Eigen::Vector3d noise_gyro_bias(noise(generator_),noise(generator_),noise(generator_)); 71 | gyro_bias_ += param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias; 72 | data.imu_gyro_bias = gyro_bias_; 73 | 74 | // acc_bias update 75 | Eigen::Vector3d noise_acc_bias(noise(generator_),noise(generator_),noise(generator_)); 76 | acc_bias_ += param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias; 77 | data.imu_acc_bias = acc_bias_; 78 | 79 | } 80 | 81 | MotionData IMU::MotionModel(double t) 82 | { 83 | MotionData data; 84 | // param 85 | float ellipse_x = 15; 86 | float ellipse_y = 20; 87 | float z = 1; // z轴做sin运动 88 | float K1 = 10; // z轴的正弦频率是x，y的k1倍 89 | float K = M_PI/ 10; // 20 * K = 2pi 　　由于我们采取的是时间是20s, 系数K控制yaw正好旋转一圈，运动一周 90 | 91 | // translation 92 | // twb: body frame in world frame 93 | Eigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5, z * sin( K1 * K * t ) + 5); 94 | 95 | Eigen::Vector3d dp(- K * ellipse_x * sin(K*t), K * ellipse_y * cos(K*t), z*K1*K * cos(K1 * K * t)); // position导数　in world frame 96 | double K2 = K*K; 97 | Eigen::Vector3d ddp( -K2 * ellipse_x * cos(K*t), -K2 * ellipse_y * sin(K*t), -z*K1*K1*K2 * sin(K1 * K * t)); // position二阶导数 98 | 99 | // Eigen::Vector3d dp(0, 0, 0); // position导数　in world frame 100 | // Eigen::Vector3d ddp( 0, 0, 0); // position二阶导数 101 | if(t<0.001) 102 | { 103 | dp = Eigen::Vector3d::Zero(); 104 | ddp = Eigen::Vector3d::Zero(); 105 | } 106 | // Rotation 107 | double k_roll = 0.1; 108 | double k_pitch = 0.2; 109 | //Eigen::Vector3d eulerAngles(k_roll * cos(t) , k_pitch * sin(t) , K*t ); // roll ~ [-0.2, 0.2], pitch ~ [-0.3, 0.3], yaw ~ [0,2pi] 110 | //Eigen::Vector3d eulerAnglesRates(-k_roll * sin(t) , k_pitch * cos(t) , K); // euler angles 的导数 111 | Eigen::Vector3d eulerAngles(k_roll * sin(t) , k_pitch * sin(t) , K*t ); // roll ~ [-0.2, 0.2], pitch ~ [-0.3, 0.3], yaw ~ [0,2pi] 112 | Eigen::Vector3d eulerAnglesRates(k_roll * cos(t) , k_pitch * cos(t) , K); // euler angles 的导数 113 | 114 | Eigen::Matrix3d Rwb = euler2Rotation(eulerAngles); // body frame to world frame 115 | Eigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates; // euler rates trans to body gyro 116 | 117 | if(t<0.001) 118 | { 119 | imu_gyro = Eigen::Vector3d::Zero(); 120 | } 121 | Eigen::Vector3d gn (0,0,-9.81); // gravity in navigation frame(ENU) ENU (0,0,-9.81) NED(0,0,9,81) 122 | Eigen::Vector3d imu_acc = Rwb.transpose() * ( ddp - gn ); // Rbw * Rwn * gn = gs 123 | 124 | data.imu_gyro = imu_gyro; 125 | data.imu_acc = imu_acc; 126 | data.Rwb = Rwb; 127 | data.twb = position; 128 | data.imu_velocity = dp; 129 | data.timestamp = t; 130 | return data; 131 | 132 | } 133 | 134 | //读取生成的imu数据并用imu动力学模型对数据进行计算，最后保存imu积分以后的轨迹， 135 | //用来验证数据以及模型的有效性。 136 | void IMU::testImu(std::string src, std::string dist) 137 | { 138 | std::vectorimudata; 139 | LoadPose(src,imudata); 140 | 141 | std::ofstream save_points; 142 | save_points.open(dist); 143 | 144 | double dt = param_.imu_timestep; 145 | Eigen::Vector3d Pwb = init_twb_; // position : from imu measurements 146 | Eigen::Quaterniond Qwb(init_Rwb_); // quaterniond: from imu measurements 147 | Eigen::Vector3d Vw = init_velocity_; // velocity : from imu measurements 148 | Eigen::Vector3d gw(0,0,-9.81); // ENU frame 149 | Eigen::Vector3d temp_a; 150 | Eigen::Vector3d theta; 151 | for (int i = 1; i < imudata.size(); ++i) { 152 | 153 | MotionData imupose = imudata[i]; 154 | 155 | //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z] 156 | Eigen::Quaterniond dq; 157 | Eigen::Vector3d dtheta_half = imupose.imu_gyro * dt /2.0; 158 | dq.w() = 1; 159 | dq.x() = dtheta_half.x(); 160 | dq.y() = dtheta_half.y(); 161 | dq.z() = dtheta_half.z(); 162 | 163 | //　imu 动力学模型　参考svo预积分论文 164 | Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw; // aw = Rwb * ( acc_body - acc_bias ) + gw 165 | Qwb = Qwb * dq; 166 | Vw = Vw + acc_w * dt; 167 | Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w; 168 | 169 | //　按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次 170 | save_points< 9 | #include 10 | #include 11 | #include 12 | 13 | #include "param.h" 14 | 15 | struct MotionData 16 | { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | double timestamp; 19 | Eigen::Matrix3d Rwb; 20 | Eigen::Vector3d twb; 21 | Eigen::Vector3d imu_acc; 22 | Eigen::Vector3d imu_gyro; 23 | 24 | Eigen::Vector3d imu_gyro_bias; 25 | Eigen::Vector3d imu_acc_bias; 26 | 27 | Eigen::Vector3d imu_velocity; 28 | }; 29 | 30 | // euler2Rotation: body frame to interitail frame 31 | Eigen::Matrix3d euler2Rotation( Eigen::Vector3d eulerAngles); 32 | Eigen::Matrix3d eulerRates2bodyRates(Eigen::Vector3d eulerAngles); 33 | 34 | 35 | class IMU 36 | { 37 | public: 38 | IMU(Param p); 39 | Param param_; 40 | Eigen::Vector3d gyro_bias_; 41 | Eigen::Vector3d acc_bias_; 42 | 43 | Eigen::Vector3d init_velocity_; 44 | Eigen::Vector3d init_twb_; 45 | Eigen::Matrix3d init_Rwb_; 46 | 47 | MotionData MotionModel(double t); 48 | void addIMUnoise(MotionData& data); 49 | void testImu(std::string src, std::string dist); // imu数据进行积分，用来看imu轨迹 50 | 51 | }; 52 | 53 | #endif //IMUSIMWITHPOINTLINE_IMU_H 54 | -------------------------------------------------------------------------------- /src/vio_data_simulation/src/param.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hyj on 17-6-22. 3 | // 4 | 5 | #include "param.h" 6 | 7 | Param::Param() 8 | { 9 | Eigen::Matrix3d R; // 把body坐标系朝向旋转一下,得到相机坐标系，好让它看到landmark, 相机坐标系的轴在body坐标系中的表示 10 | // 相机朝着轨迹里面看， 特征点在轨迹外部， 这里我们采用这个 11 | R << 0, 0, -1, 12 | -1, 0, 0, 13 | 0, 1, 0; 14 | R_bc = R; 15 | t_bc = Eigen::Vector3d(0.05,0.04,0.03); 16 | 17 | } -------------------------------------------------------------------------------- /src/vio_data_simulation/src/param.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hyj on 17-6-22. 3 | // 4 | 5 | #ifndef IMUSIM_PARAM_H 6 | #define IMUSIM_PARAM_H 7 | 8 | #include 9 | 10 | class Param{ 11 | 12 | public: 13 | 14 | Param(); 15 | 16 | // time 17 | int imu_frequency = 200; 18 | int cam_frequency = 30; 19 | double imu_timestep = 1./imu_frequency; 20 | double cam_timestep = 1./cam_frequency; 21 | double t_start = 0; 22 | double t_end = 3600 * 4; // 3600 * 4; 23 | 24 | // noise 25 | double gyro_bias_sigma = 0.00005; 26 | double acc_bias_sigma = 0.0005; 27 | 28 | //double gyro_bias_sigma = 1.0e-5; 29 | //double acc_bias_sigma = 0.0001; 30 | 31 | double gyro_noise_sigma = 0.015; // rad/s 32 | double acc_noise_sigma = 0.019; //　m/(s^2) 33 | 34 | double pixel_noise = 1; // 1 pixel noise 35 | 36 | // cam f 37 | double fx = 460; 38 | double fy = 460; 39 | double cx = 255; 40 | double cy = 255; 41 | double image_w = 640; 42 | double image_h = 640; 43 | 44 | 45 | // 外参数 46 | Eigen::Matrix3d R_bc; // cam to body 47 | Eigen::Vector3d t_bc; // cam to body 48 | 49 | }; 50 | 51 | 52 | #endif //IMUSIM_PARAM_H 53 | -------------------------------------------------------------------------------- /src/vio_data_simulation/src/utilities.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hyj on 18-1-19. 3 | // 4 | #include "utilities.h" 5 | 6 | 7 | void save_points(std::string filename, std::vector > points) 8 | { 9 | std::ofstream save_points; 10 | save_points.open(filename.c_str()); 11 | 12 | for (int i = 0; i < points.size(); ++i) { 13 | Eigen::Vector4d p = points[i]; 14 | 15 | save_points< > points, 23 | std::vector > features) 24 | { 25 | std::ofstream save_points; 26 | save_points.open(filename.c_str()); 27 | 28 | for (int i = 0; i < points.size(); ++i) { 29 | Eigen::Vector4d p = points[i]; 30 | Eigen::Vector2d f = features[i]; 31 | save_points< > features) 42 | { 43 | std::ofstream save_points; 44 | save_points.open(filename.c_str()); 45 | 46 | for (int i = 0; i < features.size(); ++i) { 47 | Eigen::Vector4d f = features[i]; 48 | save_points<& pose) 57 | { 58 | 59 | std::ifstream f; 60 | f.open(filename.c_str()); 61 | 62 | if(!f.is_open()) 63 | { 64 | std::cerr << " can't open LoadFeatures file "<>time; 86 | ss>>q.w(); 87 | ss>>q.x(); 88 | ss>>q.y(); 89 | ss>>q.z(); 90 | ss>>t(0); 91 | ss>>t(1); 92 | ss>>t(2); 93 | ss>>gyro(0); 94 | ss>>gyro(1); 95 | ss>>gyro(2); 96 | ss>>acc(0); 97 | ss>>acc(1); 98 | ss>>acc(2); 99 | 100 | 101 | data.timestamp = time; 102 | data.imu_gyro = gyro; 103 | data.imu_acc = acc; 104 | data.twb = t; 105 | data.Rwb = Eigen::Matrix3d(q); 106 | pose.push_back(data); 107 | 108 | } 109 | } 110 | 111 | } 112 | 113 | void save_Pose(std::string filename, std::vector pose) 114 | { 115 | std::ofstream save_points; 116 | save_points.open(filename.c_str()); 117 | 118 | for (int i = 0; i < pose.size(); ++i) { 119 | MotionData data = pose[i]; 120 | double time = data.timestamp; 121 | Eigen::Quaterniond q(data.Rwb); 122 | Eigen::Vector3d t = data.twb; 123 | Eigen::Vector3d gyro = data.imu_gyro; 124 | Eigen::Vector3d acc = data.imu_acc; 125 | 126 | save_points< pose) 145 | { 146 | std::ofstream save_points; 147 | save_points.setf(std::ios::fixed, std::ios::floatfield); 148 | save_points.open(filename.c_str()); 149 | 150 | for (int i = 0; i < pose.size(); ++i) { 151 | MotionData data = pose[i]; 152 | double time = data.timestamp; 153 | Eigen::Quaterniond q(data.Rwb); 154 | Eigen::Vector3d t = data.twb; 155 | Eigen::Vector3d gyro = data.imu_gyro; 156 | Eigen::Vector3d acc = data.imu_acc; 157 | 158 | save_points.precision(9); 159 | save_points < 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | // save 3d points to file 16 | void save_points(std::string filename, std::vector > points); 17 | 18 | // save 3d points and it's obs in image 19 | void save_features(std::string filename, 20 | std::vector > points, 21 | std::vector > features); 22 | 23 | // save line obs 24 | void save_lines(std::string filename, 25 | std::vector > features); 26 | 27 | 28 | void LoadPose(std::string filename, std::vector& pose); 29 | 30 | // save imu body data 31 | void save_Pose(std::string filename, std::vector pose); 32 | 33 | // save pose as TUM style 34 | void save_Pose_asTUM(std::string filename, std::vector pose); 35 | 36 | #endif //IMUSIMWITHPOINTLINE_UTILITIES_H 37 | 38 | 39 | --------------------------------------------------------------------------------