# Write VIO from scratch

Keywords: git github angular Python

Here is a summary of the second job from zero-handwriting VIO:
Homework title:

# 1 Emulation Code Parsing

In addition to the generic version of the simulation code, the github repository also provides the ROS version of the simulation code for generatingImg.bagFor later calibration using the Allan variance tool.

## 1.1 Compile vio_data_simulation-master

The first step is to compile this generic version of the emulation code, which follows the general pattern of compilation:

```git clone https://github.com/HeYijia/vio_data_simulation
cd vio_data_simulation
mkdir build
cd build
camke ..
make
cd ../bin
./data_gen
```

This produces what we need:

imu_pose_noise.txt ，imu_pose.txt ，imu_int_pose_noise.txt， imu_int_pose.txt

And so on.

Using Python scripts under python-tool, you can draw points, IMU tracks, etc.

```cd ../python_tool
python draw_trajctory.py
```

## 1.2 Simulation Thought

Specify the trajectory equation, calculate the first derivative to get the velocity, angular velocity, and second derivative to get the acceleration.The simulated data are obtained by adding Gaussian white noise and bias random walk noise to the acceleration angular velocity.

## 1.3 Motion Model

In this simulation code, theImu.cppThe motion model of IMU is defined in:

```MotionData IMU::MotionModel(double t)
{

MotionData data;
// param
float ellipse_x = 15;
float ellipse_y = 20;
float z = 1;           // Sink motion on z axis
float K1 = 10;          // The sinusoidal frequency of the z-axis is twice that of x and y
float K = M_PI/ 10;    // 20*K = 2pi

// translation world system because the position of IMU is the coordinate of body system in world system
// twb:  body frame in world frame
Eigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5,  z * sin( K1 * K * t ) + 5);
Eigen::Vector3d dp(- K * ellipse_x * sin(K*t),  K * ellipse_y * cos(K*t), z*K1*K * cos(K1 * K * t));              // position derivative in world frame
double K2 = K*K;
Eigen::Vector3d ddp( -K2 * ellipse_x * cos(K*t),  -K2 * ellipse_y * sin(K*t), -z*K1*K1*K2 * sin(K1 * K * t));     // position second derivative

// Rotation
double k_roll = 0.1;
double k_pitch = 0.2;
// Angle under body system
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]
// Deriving the Euler angle to get the Euler angle velocity in the world system
Eigen::Vector3d eulerAnglesRates(-k_roll * sin(t) , k_pitch * cos(t) , K);      // Derivatives of euler angles

//    Eigen::Vector3d eulerAngles(0.0,0.0, K*t );   // roll ~ 0, pitch ~ 0, yaw ~ [0,2pi]
//    Eigen:: Vector3d eulerAnglesRates (0., 0., K); // derivative of Euler angles

Eigen::Matrix3d Rwb = euler2Rotation(eulerAngles);         // body frame to world frame
Eigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates;   //  euler rates trans to body gyro

Eigen::Vector3d gn (0,0,-9.81);                                   //  gravity in navigation frame(ENU)   ENU (0,0,-9.81)  NED(0,0,9,81)
Eigen::Vector3d imu_acc = Rwb.transpose() * ( ddp -  gn );  //  Rbw * Rwn * gn = gs

data.imu_gyro = imu_gyro;   // Angular velocity in body system
data.imu_acc = imu_acc;     // Acceleration in body system
data.Rwb = Rwb;             //IMU position data is stored using Twb
data.twb = position;
data.imu_velocity = dp;     // IMU Speed world System
data.timestamp = t;         // time stamp
return data;

}
```

You can see that the specified IMU trajectory equation Position is relatively simple, and you can roughly think of it as a "crown" which is projected on the xy plane as a circle with a center (-5,,5).The IMU also rotates in its own coordinate system, using the Euler angle to represent its rotation, which is also a function of time.

The first derivative and the second derivative of Position are used to obtain the IMU's speed and acceleration in the world coordinate system, respectively.The first derivative of Rotation is used to obtain the angular velocity of IMU in the body system.Using these values, we finally get Motion Data for IMU:

```	data.imu_gyro = imu_gyro;   // Angular velocity in body system
data.imu_acc = imu_acc;     // Acceleration in body system
data.Rwb = Rwb;             //IMU position data is stored using Twb
data.twb = position;
data.imu_velocity = dp;     // IMU Speed world System
data.timestamp = t;         // time stamp
```

The settings are 0.019 for acceleration, 0.015 for gyroscope, 5 e_4 for acceleration bias, and 5 e_5 for gyroscope.

```void IMU::addIMUnoise(MotionData& data)
{
std::random_device rd;
std::default_random_engine generator_(rd());
std::normal_distribution<double> noise(0.0, 1.0);
// gyro
Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));// Generate a Gaussian white noise
Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();  // Feel like there's something here or not
// w = w + ng + bg, where ng = _d*n, n~N(0,1), _d = _/sqrt(_t), BG will be updated separately below
data.imu_gyro = data.imu_gyro + gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) + gyro_bias_;
// acc
Eigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_));
Eigen::Matrix3d acc_sqrt_cov = param_.acc_noise_sigma * Eigen::Matrix3d::Identity();//
// A = a + n a + ba, where n a = _d*n, n~N(0,1), _d = _/sqrt (t), Ba will be updated separately below
data.imu_acc = data.imu_acc + acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) + acc_bias_;

// gyro_bias update
Eigen::Vector3d noise_gyro_bias(noise(generator_),noise(generator_),noise(generator_));
// bg = σd*n, n~N(0,1), σd = σ*sqrt(Δt)
gyro_bias_ += param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;
data.imu_gyro_bias = gyro_bias_;

// acc_bias update
Eigen::Vector3d noise_acc_bias(noise(generator_),noise(generator_),noise(generator_));
// ba = σd*n, n~N(0,1), σd = σ*sqrt(Δt)
acc_bias_ += param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;
data.imu_acc_bias = acc_bias_;

}
```

Here we only consider Gaussian white noise and bias random walk, and we need to pay attention to their calculations:

w = w + ng + bg, among ng = σdn, n~N(0,1),σd = σ/sqrt(Δt) , bg = σdn, n~N(0,1), σd = σsqrt(Δt)
a = a + na + ba, among na = σdn, n~N(0,1),σd = σ/sqrt(Δt), ba = σdn, n~N(0,1), σd = σsqrt(Δt)

Because the data obtained by IMU are discrete, note that the variance of Gaussian white noise from continuous time to discrete time needs to be multiplied by a 1/sqrt (t), the variance of random walk noise from continuous time to discrete time needs to be multiplied by a sqrt (t), and t is the sampling time of the sensor.

So far we have obtained the simulation data of IMU, mainly the position data of IMU under the body system:
imu_pose_noise.txt
imu_pose.txt

## Discrete Integral of 1.5 Motion Model

In practice, because the frequency of IMU is much higher than that of camera, we often need to integrate discrete IMU data to align IMU data with camera data.
This time, using the discrete simulation data of IMU obtained above, Euler integral and median integral are used to compare the effect of the two integrals.(By comparing the integrated tracks imu_Int_Pose.txtWhether and the track imu_before the integralPose.txtCoincidence)

### 1.5.1 Euler Integral

Code:

```for (int i = 1; i < imudata.size(); ++i) {

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();

/// imu dynamic model Euler integral
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;

//Stored in the format imu postion, imu quaternion, cam postion, cam quaternion, since there is no cam, imu is stored twice
save_points << imupose.timestamp << " "
<< Qwb.w() << " "
<< Qwb.x() << " "
<< Qwb.y() << " "
<< Qwb.z() << " "
<< Pwb(0) << " "
<< Pwb(1) << " "
<< Pwb(2) << " "
<< Qwb.w() << " "
<< Qwb.x() << " "
<< Qwb.y() << " "
<< Qwb.z() << " "
<< Pwb(0) << " "
<< Pwb(1) << " "
<< Pwb(2) << " "
<< std::endl;
}
```

Effect:

### 1.5.2 Median Integral

Code:

```for (int i = 1; i < imudata.size(); ++i)
{

MotionData imupose_pre = imudata[i-1];
MotionData imupose_now = imudata[i];
MotionData imupose_mean = imudata[i];
imupose_mean.imu_gyro = (imupose_pre.imu_gyro + imupose_now.imu_gyro) / 2.0;

Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half = (imupose_mean.imu_gyro) * dt / 2.0;
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();

//imu dynamic model refers to svo preintegration paper
Eigen::Vector3d acc_w = Qwb * (imupose_pre.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
Qwb = Qwb * dq; //Get Qwb_k+1
//Qwb+1
Eigen::Vector3d acc_w1 = Qwb * (imupose_now.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
acc_w = (acc_w + acc_w1) / 2.0;
//a update
Vw = Vw + acc_w * dt;
//Pwb update
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;

//Stored in the format imu postion, imu quaternion, cam postion, cam quaternion, since there is no cam, imu is stored twice
save_points << imupose_mean.timestamp << " "
<< Qwb.w() << " "
<< Qwb.x() << " "
<< Qwb.y() << " "
<< Qwb.z() << " "
<< Pwb(0) << " "
<< Pwb(1) << " "
<< Pwb(2) << " "
<< Qwb.w() << " "
<< Qwb.x() << " "
<< Qwb.y() << " "
<< Qwb.z() << " "
<< Pwb(0) << " "
<< Pwb(1) << " "
<< Pwb(2) << " "
<< std::endl;
}
```

Effect:

Obviously, the median integral works better than the Euler integral. Of course, this is obvious, and we can see from the principle that:

# IMU Emulation Code for 2ROS Version

```mkdir vio_sim_ws/src
cd src
git clone https://github.com/HeYijia/vio_data_simulation
git checkout ros_version
catkin_make

```

## 2.2 GenerationImu.bag

First, open catkin_ws_vio_data_simulation/src/vio_data_simulation-ros_version/src/gener_alldata.cpp, settingImu.bagStorage path.

bag.open("/your-path/imu.bag", rosbag::bagmode::Write);

Then, start the node, generateImu.bag.

```source devel/setup.bash
rosrun vio_data_simulation vio_data_simulation_node
```

At this point, we get a ROS bag package of IMU simulation data.Next, it is calibrated using the Allan variance tool.

# 3 Calibration using the Allan variance tool

Mainly using imu_utils and kalibr_allan to calibrate.

## 3.1 Use imu_utils calibration

### 3.1.1 Install im_utils

First, the installation depends on:

```sudo apt-get install libdw-dev
```

Compile code_firstUtils, then compile imu_utils

```mkdir -p imu-calibration/src
cd imu-calibration/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make
cd imu-calibration/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make
```

Intermediate problems:
We usually install Eigen with the following commands:

```sudo apt-get install libeigen3-dev
```

Eigen is then installed by default in / usr/include/eigen3, requiring / home/jlg/imu-calibration/src/code_Utils/CMakeLists.txtComment out find_inPackage (Eigen3 REQUIRED), then add:

include_directories(/usr/include/eigen3)

atal error: backward.hpp: No such file or directory

Put file code_utils/src/sumpixel_test.cpp#include inBackward.hpp"Change to #include" code_Utils/Backward.hpp"Yes.

Error 3:std::ofstream undefined

/home/***/imu-calibration/src/imu_utils/src/imu_an.cpp:69:19: error:
aggregate 'std::ofstream out_t' has incomplete type and cannot be
defined

```#include <fstream>
```

Reference here:https://blog.csdn.net/learning_tortosie/article/details/102415313
In fact, it may be lucky that I don't have these problems, but keep a record just in case.

### 3.1.2 Calibration

1. Collecting IMU data
The IMU data here uses what we generated aboveImu.bag.Of course, the author also provided several packages, you can test them first.

As you can see, we generated theImg.bagTopic is imu, about four hours.
2. Write launch file
You can refer to several launch files written by the author under the Launch folder to modify them according to the topic and name of your imu:
```<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= "mems"/>
<param name="data_save_path" type="string" value= "\$(find imu_utils)/data/"/>
<param name="max_time_min" type="int" value= "120"/>
<param name="max_cluster" type="int" value= "100"/>
</node>
</launch>

```

For example, I modified imu_hereTopic corresponding value:/imu, imu_Value:mems for name, and remember to change the launch file name to imu_name, here I amMems.launch.

1. Generate Allan variance data
Start the launch file just now
```cd imu-calibration
source./devel/setup.bash
roslaunch imu_utils mems.launch
```

Open a new terminal window to play at 200X speedImu.bag

```rosbag play -r 200 imu.bag
```

Notice hereImg.bagTo take the path you store: yourpath/imu.bag

After a while (around 15s?), some data will be generated under the data folder:

And a mems_imu_param.yaml:

```%YAML:1.0
---
type: IMU
name: mems
Gyr:
avg-axis:
gyr_n: 2.1221203233760574e-01
gyr_w: 8.9467924142534435e-04
x-axis:
gyr_n: 2.1046384179740413e-01
gyr_w: 8.0473827758366584e-04
y-axis:
gyr_n: 2.1360563530391058e-01
gyr_w: 9.2214122848867288e-04
z-axis:
gyr_n: 2.1256661991150250e-01
gyr_w: 9.5715821820369432e-04
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 2.6764053623067113e-01
acc_w: 3.6174135533268148e-03
x-axis:
acc_n: 2.6811296473220231e-01
acc_w: 3.6652237104732588e-03
y-axis:
acc_n: 2.6789918945541225e-01
acc_w: 3.7987720199202969e-03
z-axis:
acc_n: 2.6690945450439874e-01
acc_w: 3.3882449295868896e-03

```
1. Draw Allan variance map
Modify draw_File path in allan.m

Change these paths to the path of the data you just generated and run draw_Allan.m

It is important to note that the unit of ordinate coordinates is deg/h.
For an understanding of this diagram, you can refer to:
https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf
https://www.vectornav.com/support/library/gyroscope
Gauss white noise and Walking Bias error of acceleration and angular velocity can be obtained by analyzing the graph
When t =1, the vertical coordinate value corresponding to the cable is Gaussian white noise
The corresponding ordinate at t=3 is the wandering Bias error.

Posted by twistedmando on Wed, 17 Jun 2020 09:34:00 -0700