自动驾驶与机器人中的slam技术（高翔）第四章（预积分和图优化）run_gins_pre_integ.cc代码笔记

# W为世界坐标，G为GNSS坐标，B为本体坐标，以IMU位置为准。
# 状态量 x = [p,v,R，bg，ba，g].transpose
# 误差量 dx_ = [p,v,Log(R)，bg，ba，g].transpose
# 预积分，回环检测时需要更新状态量，如果直接积分，则全部数据都需要更改了，使用预积分把测量值和状态量分开，则可避免了
    预积分观测量需要加上噪声量（零均值高斯分布）才是真实的，如果测量噪声为0，则噪声为0，再加上GNSS之前测得数据，则是真实数据了。
# 旋转量：theta*a，J = sin(theta)/theta * I + (1-sin(theta)/theta)*aaT + (1 - cos(theta))/theta *a^
# GNSS值得到的是相对与最开始的原点的
初始化：零偏：bg_ = 0; ba_ = 0; 预积分观测量：dp_ = 0; dv_ = 0; dR_ = 0;零偏噪声协方差矩阵：noise_gyro_acce_ 自定义;
累计噪声协方差矩阵：cov_ = Mat9d::Zero(); current_time_记录每次数据的最新时间   

1、读取odom的轮速，判断是否静止
2、如果imu未初始化且静止了，记录静止后第一次imu出现的时间t1。记录从t1开始持续t2时间的imu数据进行初始化
3、使用imu静止时的平均加速度作为重力加速度，imu再加上重力，因为imu测量到的是反向重力
4、使用各个方向计算的均值作为零偏ba、bg
5、第一次gnss以及初始化得到的bg，ba作为last_frame的状态量[t，p,v,R，bg，ba]
6、第一次IMU作为last_imu_
7、再接受到IMU，矫正IMU测量值gyr = imu.gyro_ - bg_;acc = imu.acce_ - ba_;
8、计算中间值：
    Vec3d omega = gyr * dt;         // 转动量
    Mat3d rightJ = SO3::jr(omega);  // 右雅可比
    SO3 deltaR = SO3::exp(omega);   // exp后

9、更新噪声项的协方差（测量噪声导致的，衡量噪声项的偏差程度）：
            [deltaR.matrix().transpose()          0    0                [rightJ * dt   0
    A   =   -dR_.matrix() * dt * acc^            I    0      B =        0       dR_.matrix() * dt
            -0.5f * dR_.matrix() * acc^ * dt*dt  dt*I I]                0       0.5f * dR_.matrix() * dt*dt  ]
    cov_ = A * cov_ * A^T + B * noise_gyro_acce_ * B^T
10、更新预积分观测量（增加imu新的数据）：
    dp_ = dp_ + dv_ * dt + 0.5f * dR_.matrix() * acc * dt * dt; 
    dv_ = dv_ + dR_ * acc * dt;
    dR_ = dR_ * deltaR；
11、更新零偏对预积分观测值的影响，即观测值对零偏的偏导数，用于零偏改变时，更新观测值：
    dP_dba_ = dP_dba_ + dV_dba_ * dt - 0.5f * dR_.matrix() * dt2;                      // (4.39d)
    dP_dbg_ = dP_dbg_ + dV_dbg_ * dt - 0.5f * dR_.matrix() * dt2 * acc_hat * dR_dbg_;  // (4.39e)
    dV_dba_ = dV_dba_ - dR_.matrix() * dt;                                             // (4.39b)
    dV_dbg_ = dV_dbg_ - dR_.matrix() * dt * acc_hat * dR_dbg_;   
    dR_dbg_ = deltaR.matrix().transpose() * dR_dbg_ - rightJ * dt;  // (4.39a)
12、累积预积分所用的时长dt_ += dt;
13、重复7-12，直到接受到GNSS
14、使用last_imu的数据预积分到GNSS时间
15、使用上一次预测的状态last_frame,以及预积分观测值预测这一次的状态量this_frame.
    this_frame.Rj = start.R_ * dR_; 
    this_frame.vj = start.R_ * dv_ + start.v_ + grav * dt_;
    this_frame.pj = start.R_ * dp_ + start.p_ + start.v_ * dt_ + 0.5f * grav * dt_ * dt_;
    this_frame.bg_= bg_;
    this_frame.ba_= ba_;
16、使用g2o进行图优化，矫正各个参数
    （高斯牛顿法（在平方为展开的情况下求导）补充：
    1、计算误差e
    2、求出雅可比矩阵J，误差对需要更新的状态量的偏导数
    3、计算海塞矩阵H = J*J.transpose()
    4、根据H * dx = -J*e求出dx
    5、更新状态量x=x+dx）
（1）添加顶点:依次添加last_frame和this_frame的pose, v, bg, ba作为g2o观测值，
    顶点需要四个函数read、write、setToOriginImpl、oplusImpl，
    一般read和write函数是数据输入输出的，可以不实现，
    setToOriginImpl函数是设置顶点初始值，一般是自己调用，而不是g2o调用，
    oplusImpl函数是更新顶点，传入值类似于高斯牛顿里的dx，根据dx更新顶点，这个函数需要实现如何更新状态量的
（2）添加边：
    1）顶点：last_frame的pose, v, bg, ba和this_frame的pose, v
    2）顶点：last_frame.bg_和this_frame.bg_
    3）顶点：last_frame.ba_和this_frame.ba_
    4）顶点：last_frame的pose, v, bg, ba，
    5）顶点：last_gnss
    6）顶点：this_gnss
    7）顶点：根据odom计算出来的世界坐标系的速度作为last_frame的v的观测值
    边需要四个函数read、write、computeError、linearizeOplus
    一般read和write函数是数据输入输出的，可以不实现，
    computeError是计算顶点的误差，即高斯牛顿里的e
    linearizeOplus是计算雅可比矩阵J，即高斯牛顿里的J
17、运行optimizer.optimize(20)，迭代20次求解。
    迭代步骤：
    （1）依次运行边的computeError函数，求出每条边的误差e，即高斯牛顿法里的e
    （2）依次运行边的linearizeOplus函数，求出每条边的雅可比矩阵J
    （3）查看高斯牛顿可知，知道了e和J，只要套公式就可以求出高斯牛顿里的dx，求dx这一步由g2o自己实现
    （4）将求到的dx传入顶点的oplusImpl函数，更新状态量x = x+dx（旋转量的话，为R*dR）
18、结束一轮操作，如果继续接收到IMU，则继续预积分，如果是gnss，则使用图优化优化状态量。