1, Basic work, required

Environment configuration description

a. ROS environment construction

For the installation and configuration of ros, refer to the following blog:

• ROS installation

• Problems encountered in ROS installation

If the rospack package cannot be found, you can execute the following command to add the package to the environment variable (note that the path should be changed to your own path).

source /home/ubuntu/catkin_ws/devel/setup.bash

Or write directly bashrc file

b. Matlab installation

Reference documents for matlab:

• This article is mainly about learning to use to view the Topic information of a bag

• How to execute matlab code

Execute script first_ allan_ Matparrallel, and then execute SCRIPT_process_results

If there is a lack of Parallel Computing Toolbox and a lack of package, click Parallel Computing Toolbox directly to jump to the installation interface j for installation (you can go to the next step before performing this step, otherwise there may be a problem that you can't write). If you can't jump, click here Installation instructions conduct.

Problems that cannot be written during installation: [ubuntu Matlab installation toolbox] unable to write / usr/local/MATLAB/R2018b] for reference This blog

1. Set different parameters in IMU simulation code to generate Allen variance calibration curve.

Simulation code: https://github.com/HeYijia/vio_data_simulation (non ros version)

allan variance tool:

​ https://github.com/gaowenliang/imu_utils
​ https://github.com/rpng/kalibr_allan

  the course will provide a Course2_ hw_ The compressed package of new contains two folders, including vio_data_simulation is used to generate motion imu data. There is also code for drawing in this folder, which is mainly used in part a below. And vio_data_simulation-ros_ The version folder is used to generate static imu simulation data of ROS version, which is mainly used in part b below.

course2_hw_new
├── readme.md
├── vio_data_simulation
└── vio_data_simulation-ros_version

a. For non ROS: generate motion imu data

Code debugging:

Mainly perform the following steps (Euler integral is used by default):

1. compile

   In Vio_ data_ Under the simulation folder, execute the following command:

   bash build.sh
   

   Compiled successfully:
   

2. Execute bin/data_gen, generate data

   Switch the directory to the bin folder and run datagen to generate a lot of data, as shown in the following figure:
   

3. Execute python_tools/draw_trajectory.py, the following diagram of real track and imu track will appear.
   
   The part of replacing Euler integral with median integral will be described in question 2 below.

Code understanding:

Code directory core code:

vio_data_simulation
├── bin(Store executable code, which will be generated after compilation data_gen Executable file for generating motion imu (data)
│   ├── house_model(It's stored inside house The key points of the model, a total of 23 point pairs, six-dimensional data (should be two points) x,y,z Coordinates), used to generate point lines)
├── main
│   └── gener_alldata.cpp(Used to generate all data, and finally txt Documents, including imu Pose, camera pose and other data)
├── python_tool
│   ├── draw_points.py(Dynamically draw the connection between the motion trajectory and the house model, which needs to be used when running python2 Execute, otherwise it will be reported map (error of)
│   ├── draw_trajcory.py(Draw the real trajectory and imu Line of motion track)
│   ├── GeometryLib.py( Some functions of geometric transformation, Euler angle,　Rotation matrix, etc.)
│   └── transformations.py
├── src
│   ├── imu.cpp(contain IMU Motion model part and test IMU,add to IMU Noise (equal function)
│   ├── param.cpp(camera Rotation matrix to world coordinate system R)
│   ├── utilities.cpp((including functions such as loading pose, saving pose, saving line, saving point, etc.)

Run draw_points.py code, the following figure can be obtained:

It can be seen that IMU should move sinusoidally along the Z axis around the house (the angle of view in the screenshot here is not adjusted well, but it can also be seen that it fluctuates up and down).

In imu In the CPP code, the testImu function reads the generated imu data, calculates the data with the imu dynamic model, and finally saves the track after imu integration. The core code calculated by Euler integration is as follows:

 MotionData imupose = imudata[i];

//delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half =  imupose.imu_gyro * dt /2.0;
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
dq.normalize();

///Euler integral of imu dynamic model
Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
Qwb = Qwb * dq;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = Vw + acc_w * dt;

The code here corresponds to the Euler integral formula provided in hobo course (as shown in the figure below). Since g is a negative value, the code above is + gw.

b. For ROS: static imu data is specially generated for allan variance calibration

  first, let's talk about which work packages need to be compiled and how to compile them. In this topic, a teacher of he family provided two methods to complete allan calibration of static imu data. One is gaowenliang IMU of_ Utils function package. One is to use kalibr_allan function package for calibration. The two function packages can be compiled in different workspaces or in one workspace. Too much space is too messy (but this method is relatively simple and not easy to make mistakes), so I will compile in a space below.

Using imu_utils completes allan calibration

1. Compiling under ros

  in IMU_ README for utils As you can see from the MD file, compiling this function pack requires:

• Install dependent Libraries

sudo apt-get install libdw-dev

• download code_utils , add ros function pack code_utils and imu_utils is named catkin_ws workspace, and then catkin_make compile operation.

Create folder catkin_ws/src, and then download code_utils.

git clone https://ghproxy.com/https://github.com/gaowenliang/code_utils

Note: there may be three or four points to pay attention to in the compilation here. It is recommended to take a look first This blog , the pit can be avoided in advance by making corresponding modifications.

Code compiled successfully_ Utils function package, screenshot is as follows:

2. Execute to generate IMU bag

• Provide the course with a vio_ data_ simulation-ros_ Put the version folder into the src folder and execute catkin_ Compile with the make command. vio_data_simulation_node is used to generate data.
  

• Add the function package name to the environment variable of the current terminal, or directly write to the configuration file of the environment variable. Note that the path below is changed to your own path.

   source /home/ximing/code/vio_code/ch2/catkin_ws/devel/setup.bash
  

• Run the data to generate node and IMU Bag data, the path is / home / Ximing / IMU bag

   rosrun vio_data_simulation vio_data_simulation_node
  

  

1. rosbag play -r 500 imgimu_utils.bag playback

   Execute rosbag info / home / Ximing / imu Bag view imu From the information of bag, we can see that the topic information is imu:
   

   It needs to be in imu_utils/launch add an IMU Launch file:

   <launch>
       <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
           <param name="imu_topic" type="string" value= "imu"/>
           <param name="imu_name" type="string" value= "simu_imu"/>
           <param name="data_save_path" type="string" value= "$(find imu_utils)/analysis_imu_data/"/>
           <param name="max_time_min" type="int" value= "120"/>
           <param name="max_cluster" type="int" value= "100"/>
       </node>
   </launch>
   

   Run the calibration tool imu_utils, using imu_utils to receive and analyze, execute the following commands:

   source /home/ximing/code/vio_code/ch2/catkin_ws/devel/setup.bash
   roslaunch imu_utils imu.launch
   

   Start another terminal and execute the following command:

   source /home/ximing/code/vio_code/ch2/catkin_ws/devel/setup.bash
   rosbag play -r 500 /home/ximing/imu.bag
   

   The operation process is shown in the figure below:
   

2. Use imu_utils for reception and analysis

   After executing the two commands in the above 3, it will be displayed in analysis_ imu_ 17 txt files are generated in the data folder. These documents are the measured errors.

3. Use IMU_ scripts under utils / matlab script under draw allan curve

   Modify the matlab script file draw allan. M code, change the path to the created analysis_imu_data, run the program to obtain the following Allan curve:

   

Using kalibr_allan completes allan calibration (recommended)

1. Compiling under ros

   take kalibr_allan Feature Pack Put it in the src file, go back to the workspace and catkin_make compilation

2. Execute to generate IMU bag

   This step can be omitted when using imu_utils has been generated.

3. Using kalibr_allan's bagconver ter converts the bag into a mat file. See readme

   Execute the following command:

   rosrun bagconvert bagconvert /home/ximing/imu.bag imu
   

   

4. Using kalibr_allan's M script analyzes the mat file (you need to modify the mat file path in the m file)

   Using matlab software, run SCRIPT_allan_matparallel.m script file

   

   After running, in kalibr_ Generate a file in the Allan / data folder mat file.

5. Using kalibr_allan's M script draws an allan curve (the path of the result file in the m file needs to be modified)

   Modify the mat file path to kalibr_ In the Allan / data folder Mat file, running SCRIPT_process_results.m script file, Allan variance calibration curves of accelerometer and gyroscope will be obtained

   The error result is:

   accelerometer_noise_density = 0.01910187
   accelerometer_random_walk   = 0.00054711
   gyroscope_noise_density     = 0.01518023
   gyroscope_random_walk       = 0.00004984
   

   The Allan variance calibration curve of accelerometer and gyroscope is shown in the figure below:
   

2. Replace Euler integral in IMU simulation code with median integral.

The idea of median integral and Euler integral:

   Euler method, that is, the pose of two adjacent times k to k+1 is the measured value a at time k, ω To calculate, the median method, that is, the pose from two adjacent times k to k+1 is the measured value a at two times, ω The average of. Here we borrow two hand-painted drawings from he Bo to better illustrate and understand:

Core part of code modification:

 ///Median integral of imu dynamic model
 MotionData imupose = imudata[i];
 MotionData imuposeK = imudata[i-1];

//delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half =  ((imupose.imu_gyro + imuposeK.imu_gyro)/2.0) * dt /2.0;
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
dq.normalize();

Eigen::Vector3d acc_w = (Qwb * (imupose.imu_acc) + gw + Qwb * (imuposeK.imu_acc) + gw)/2.0;  // aw = Rwb * ( acc_body - acc_bias ) + gw
Qwb = Qwb * dq;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = Vw + acc_w * dt;

(pit avoidance tip: if the code is modified based on the Euler integral above, manually delete the files in the build folder before recompiling, otherwise an error may occur.)

The operation results are shown in the figure below. The yellow line (trajectory) and the blue line (actual motion trajectory) are highly coincident. Compared with Euler integral, the integration result is more accurate.

2, Lifting operation, optional

1. Read the paper on generating imu data from existing tracks, and write a summary and derivation:

To be updated