vio第二章-中值积分修改

1、安装库
安装sophus
因之前安装的是eigen3.2.29，重新源码安装了eigen3.3.4
编译sophus还是报错，参考：https://www.it610.com/article/1290749043999318016.htm 这篇文章。

其他可以参考：https://www.it610.com/article/1281501671595196416.htm。

安装ceres
由于上面安装的是eigen3.3版本，因此在ceres下载最新的版本的，进行编译。

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.4 libgflags-dev
sudo apt-get install libgoogle-glog-dev libgtest-dev

//下载最小的ceres
git clone https://github.com/ceres-solver/ceres-solver

由于我的glog问题，我在编译时，指定了MINIGLOG

cmake  -DMINIGLOG=ON ..
make -j4
sudo make install

2、欧拉积分改为中值积分看效果
先做个小总结：
imu可以给出采样时刻的基于本体坐标系的角速度、加速度，而我们需要的是这个imu现在在W坐标系下的姿态情况，包括速度、旋转矩阵、位置。
怎么进行处理？
首先imu得到的角速度、加速度有关联吗？为什么要得到角速度和加速度，只有加速度行吗？只有角速度行吗？角速度和磁力计结合行吗？
这个角速度和加速度是很有必要的一对数据，加速度是需要依赖角速度才能转换到W坐标系下的。
通过角速度对时间积分，可以得到一段时间内的旋转了多少度（三个轴的），与上一时刻的旋转矩阵相乘，就可以得到当前时刻imu坐标系相对于W坐标系的旋转情况Rwb,有了这个Rwb，就可以把采样到的imu坐标系下的加速度转换到W坐标系下，从而积分出速度、位移。
刚才说的旋转角度对时间积分，说的是理想的已知角速度变化曲线情况下的积分，当然是很精准的可以知道一段时间的实际旋转角度。但实际上，我们并不知道imu会怎么旋转，所以也就无从知道这个imu旋转角度曲线公式（除非是自己造数据，指定了路径），拿到都是一个个离散的，当前测量到的旋转速度（角速度）。如何能把上一时刻到当前时刻这段时间的旋转角度计算的更准确些？一个粗暴的做法就是直接认为从上一时刻起，其角速度都保持与当前得到的角速度一样的值，这个就是欧拉积分的做法。可想而知除非真是匀转速情况，否则是不准确的。另一种，就是认为上一时刻到当前时刻，其角速度变化是线性的，取中间值来代替。这个确实可以更准确些。
要想更准确，可以让采样周期更短。

下面是欧拉积分改为中值积分的代码：

//delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
        Eigen::Quaterniond dq;
        #ifdef Eura_method
        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();
        
        // imu 动力学模型 欧拉积分
        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;
        
        #else
        /// 中值积分
        Eigen::Vector3d dtheta_half =  (imupose.imu_gyro+imudata[i-1].imu_gyro)/2 * dt /2.0;
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        dq.normalize();

		Qwb = Qwb * dq;
        Eigen::Vector3d acc_w=(Qwb*(imupose.imu_acc)+gw+pre_Qwb*(imudata[i-1].imu_acc)+gw)/2;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = Vw + acc_w * dt;
        #endif

原来的采样频率为200，基本上看不出中值积分与真值的差别，欧拉积分的差别是很明显的，将频率改为50，看看：

欧拉积分：

中值积分：

差距很明显。