SLAM中的非线性优化-2D图优化之三轴IMU预积分前传（五）

原创已于 2025-04-06 09:10:32 修改 · 845 阅读

23 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

收录于

SLAM中非线性优化的从古至今

于 2025-04-05 21:24:44 首次发布

本节来补充上一讲，继续讲解直接积分的图优化问题该怎么设计；相关说明请看仓库内的README.md文件

本篇博客对应完整代码放置在个人仓库：https://gitee.com/zl_vslam/slam_optimizer/tree/master/2d_optimize/ch4

一、位姿图表示

本节以两个imu时刻的状态以及对应的观测来表示优化问题，如下所示：

上图中蓝色矩形为相邻两时刻分别对应的观测，下方圆圈表示相邻两时刻分别对应的状态，积分方法参考上一讲《SLAM中的非线性优化-2D图优化之三轴IMU预积分（四）》，其中x1与x2连线可看作imu积分形成的边，绿色矩形框可认为是轮速里程计形成的边，不过这两条边本讲不做讨论

二、状态

由于涉及两个时刻的状态，优化过程中也需要同时优化两时刻的状态；简化系统估计状态如下

x=(x1, x2)； x1=(p1, v1, theta1, ba1, bg1) ; 8x1大小的向量; x2=(p2,v2,theta2) ; 5x1大小

三、观测

P=(p3, p4）; p3=(p,v,theta); 5x1大小; p4=(p,v,theta); 5x1大小

四、残差项

$e=\frac{1}{2}(e_{13}^{2}+e_{24}^{2})$ ，其中残差项分别为

$e_{13}=((p_{1}-p_{3}), (v_{1}-v_{3}), (\theta _{1}-\theta _{3}))$

$e_{24}=((p_{2}-p_{4}), (v_{2}-v_{4}), (\theta _{2}-\theta _{4}))$

五、雅可比矩阵

$J=\begin{bmatrix} J_{13}\\ J_{24} \end{bmatrix}$

其中

$J{_{13}}=\frac{\partial e_{13}}{\partial x}=(\frac{\partial e13}{\partial x1},\frac{\partial e13}{\partial x2})$

$J{_{24}}=\frac{\partial e_{24}}{\partial x}=(\frac{\partial e24}{\partial x1},\frac{\partial e24}{\partial x2})$

接下来分别对雅可比矩阵展开求解，

5.1 先求e13关于x1的雅可比矩阵；分别对状态求解

(1)先位置对位置求导

$\frac{\partial e13}{\partial x1}=\frac{\partial e13}{\partial p1}=\frac{\partial (p1-p3))}{\partial p1}=I_{2\times 2}$

(2)再对位置对速度求导

$\frac{\partial e13}{\partial x1}=\frac{\partial e13}{\partial v1}=\frac{\partial (p1-p3))}{\partial v1}=0_{2\times 2}$

(3)再对位置对角度求导

$\frac{\partial e13}{\partial x1}=\frac{\partial e13}{\partial \theta 1}=\frac{\partial (p 1-p 3))}{\partial \theta 1}=0_{2\times 1}$

(4)位置对加速度偏置求导

$\frac{\partial e13}{\partial x1}=\frac{\partial e13}{\partial ba1}=\frac{\partial (p1-p3))}{\partial ba1}=0_{2\times 2}$

(5)位置对角速度偏置求导

$\frac{\partial e13}{\partial x1}=\frac{\partial e13}{\partial bg1}=\frac{\partial (p1-p3))}{\partial bg1}=0_{2\times 1}$

如上述求导所示，再利用速度对位置/速度/角度/加速度偏置/角速度偏置分别求导，同样求法，只在速度部分是个单位矩阵，其余全为0。

角度一样，只在对自己求导部分是1，其余全为0，因此

$\frac{\partial e13}{\partial x1}=\begin{bmatrix} I_{5\times 5} & 0_{5\times 3}\end{bmatrix}$

5.2 再求e13关于x2的雅可比矩阵；由于上一时刻的状态于下一时刻无关，因此此处的雅可比为全0

$\frac{\partial e13}{\partial x2}=0_{5\times 5}$

5.3 再求e24关于x1的雅可比矩阵；当前状态与上一时刻相关，因此

(1-0)先位置对位置求导

$\frac{\partial e24}{\partial x1}=\frac{\partial e24}{\partial p1}=\frac{\partial (p2-p4))}{\partial p1}=I_{2\times 2}$

将上一讲公式（1）位置更新带入p2，则可轻易求得雅可比

(1-1)再对位置对速度求导

$\frac{\partial e24}{\partial x1}=\frac{\partial e24}{\partial v1}=\frac{\partial (p2-p4))}{\partial v1}=dt\ \cdot I_{2\times 2}$

(1-2)再对位置对角度求导, 带入上一讲公式（1）并省略掉无关项

$\frac{\partial e24}{\partial x1}=\frac{\partial e24}{\partial \theta 1}=\frac{\partial (p 2-p 4))}{\partial \theta 1}=\frac{\partial (\frac{1}{2}\cdot R_{1}\cdot (a_{b1}-b_{a1})\cdot dt^{2})}{\partial \theta _{1}}$

只有旋转矩阵与角度相关

$devR=\frac{\partial R}{\partial \theta }=\begin{bmatrix} 0 & -1\\ 1& 0 \end{bmatrix}\cdot R$

因此

$\frac{\partial e24}{\partial \theta 1}=\frac{1}{2}\cdot devR_{1}\cdot (a_{b1}-b_{a1})\cdot dt^{2})$

(1-3)位置对加速度偏置求导, 带入上一讲公式（1）并省略掉无关项

$\frac{\partial e13}{\partial x1}=\frac{\partial e24}{\partial ba1}=\frac{\partial (p2-p4))}{\partial ba1}={\frac{1}{2}}\cdot R_{0}\cdot dt^{2}$

(1-4)位置对角速度偏置求导

$\frac{\partial e24}{\partial x1}=\frac{\partial e24}{\partial bg1}=\frac{\partial (p2-p4))}{\partial bg1}=0_{2\times 1}$

(2-0)速度对位置求导

$\frac{\partial e24}{\partial x1}=\frac{\partial e24}{\partial p1}=\frac{\partial (v2-v4))}{\partial p1}=0_{2\times 2}$

(2-1)速度对速度求导, 带入上一讲公式（2）并省略掉无关项

$\frac{\partial e24}{\partial x1}=\frac{\partial e24}{\partial v1}=\frac{\partial (v 2-v 4))}{\partial \ v1}=I_{2\times 2}$

(2-2)速度对角度求导, 带入上一讲公式（2）并省略掉无关项

$\frac{\partial e24}{\partial x1}=\frac{\partial e24}{\partial \theta 1}=\frac{\partial (v 2-v 4))}{\partial \theta 1}=\frac{\partial (\cdot R_{1}\cdot (a_{b1}-b_{a1})\cdot dt)}{\partial \theta _{1}}$

$\frac{\partial e24}{\partial \theta 1}= devR_{1}\cdot (a_{b1}-b_{a1})\cdot dt)$

(2-3)速度对加速度偏置求导, 带入上一讲公式（2）并省略掉无关项

$\frac{\partial e13}{\partial x1}=\frac{\partial e24}{\partial ba1}=\frac{\partial (v2-v4))}{\partial ba1}= R_{0}\cdot dt$

(2-4)速度对角速度偏置求导

$\frac{\partial e24}{\partial x1}=\frac{\partial e24}{\partial bg1}=\frac{\partial (v2-v4))}{\partial bg1}=0_{2\times 1}$

(3-0)角度对位置求导

$\frac{\partial e24}{\partial x1}=\frac{\partial e24}{\partial p1}=\frac{\partial (\theta 2-\theta 4))}{\partial p1}=0_{1\times 2}$

(3-1)角度对速度求导, 带入上一讲公式（3）并省略掉无关项

$\frac{\partial e24}{\partial x1}=\frac{\partial e24}{\partial v1}=\frac{\partial (\theta 2-\theta 4))}{\partial \ v1}=0_{1\times 2}$

(3-2)角度对角度求导, 带入上一讲公式（3）并省略掉无关项

$\frac{\partial e24}{\partial x1}=\frac{\partial e24}{\partial \theta 1}=\frac{\partial (\theta 2-\theta 4))}{\partial \theta 1}=1$

(3-3)角度对加速度偏置求导, 带入上一讲公式（3）并省略掉无关项

$\frac{\partial e13}{\partial x1}=\frac{\partial e24}{\partial ba1}=\frac{\partial (\theta 2-\theta 4))}{\partial ba1}= 0_{1\times 2}$

(3-4)角度对角速度偏置求导

$\frac{\partial e24}{\partial x1}=\frac{\partial e24}{\partial bg1}=\frac{\partial (\theta 2-\theta 4))}{\partial bg1}=dt$

5.4 再求e24关于x2的雅可比矩阵；上一讲已经求解过了，但是此处x2，偏置不在状态中，因此

$\frac{\partial e13}{\partial x2}=I_{5\times 5}$

将上述雅可比横着放入对应位置即可

六、算法实现

主调用流程

int main() {  
	double acc_noise_std = 0.1;
	double init_bias = 0.0;
	std::vector<double> timestamps;
	std::vector<Eigen::Vector3d> imu_data;  
	std::vector<Eigen::Vector3d> odometry_data;
	std::vector<Eigen::Vector3d> vel_data;  
	std::vector<Eigen::Vector3d> pose_data;
	std::vector<Eigen::Vector3d> gps_data;
	read_simulate(init_bias, timestamps, imu_data, odometry_data, vel_data, pose_data, gps_data);

	ImuIntegration imu_integration(init_bias);

    State state;  
    memset(&state, 0.0, sizeof(state));
    std::vector<Eigen::Vector3d> pose_gt = pose_data;
    double linear_vel = 0.5;
    double true_vx = linear_vel * std::cos(pose_gt[0][2]);
    double true_vy = linear_vel * std::sin(pose_gt[0][2]);
    // state.p = Eigen::Vector2d(gps_data[0][1], gps_data[0][2]);
    state.v = Eigen::Vector2d(true_vx,true_vy);
    // state.theta = 0.001;
    state.ba = Eigen::Vector2d(1e-6,1e-6);
    state.bg = init_bias;

    State last_state = state; 

    Optimizer optimizer;
    bool is_using_ceres = false;
	for(unsigned int i = 0; i < timestamps.size()-1; i++) {

		const Eigen::Vector3d &last_imu = imu_data[i];
		const Eigen::Vector3d &curr_imu = imu_data[i+1];

		// imu state and cov update
		imu_integration.Integrate(true, last_imu, curr_imu, timestamps[i+1]-timestamps[i], state);

		Eigen::Vector3d last_odometry = odometry_data[i];
		Eigen::Vector3d curr_odometry = odometry_data[i+1];
		Eigen::Vector3d delta_odometry = curr_odometry - last_odometry;

		Eigen::Vector3d last_velocity = vel_data[i];
		Eigen::Vector3d curr_velocity = vel_data[i+1];
		
		Eigen::Vector3d last_pose = pose_data[i];
		Eigen::Vector3d curr_pose = pose_data[i+1];
		if(is_using_ceres) {
			CeresOptimizer::OptimizeGN(last_state, last_pose, last_velocity.head<2>(), state, curr_pose, curr_velocity.head<2>());
		} else {
			// optimizer.OptimizeGN(last_state, state, last_velocity, curr_velocity, delta_odometry, last_pose, curr_pose);
			optimizer.OptimizeLM(last_state, state, last_velocity, curr_velocity, delta_odometry, last_pose, curr_pose);
		}


		last_state = state;

		usleep(150000);
	}

	return 0;
}

注意此处用了LM算法，并利用Ceres以及自己实现了数值微分来验证雅可比正确性，数值导跟解析导数之间只在5.3处发生了不一致，其余地方完全一样，并且算法采用LM后，结果收敛到真值上；

雅可比计算与推导一模一样

void PoseObserve::ComputeResidualAndJacobianPoseGraph(const State& last_state, const Eigen::Vector3d &last_pose, const Eigen::Vector2d &last_velocity, const State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity, Eigen::Matrix<double, 5, 1> &residualXi, Eigen::Matrix<double, 5, 8> &jacobianXi, Eigen::Matrix<double, 5, 1> &residualXj, Eigen::Matrix<double, 5, 5> &jacobianXj, Eigen::Matrix<double, 5, 5> &jacobianXij, Eigen::Matrix<double, 5, 8> &jacobianXji) {
	PoseObserve::ComputeResidual(last_state, last_pose, last_velocity, residualXi);
	PoseObserve::ComputeResidual(state, pose, velocity, residualXj);

	jacobianXi = Eigen::Matrix<double, 5, 8>::Zero();
	jacobianXi.block<5,5>(0,0) = Eigen::Matrix<double, 5, 5>::Identity();
	
	jacobianXij = Eigen::Matrix<double, 5, 5>::Zero();

	Eigen::Matrix<double,2,2> R = math_utils::rotation(last_state.theta);

    Eigen::Vector3d last_imu_unbias(state.last_imu[0], state.last_imu[1], state.last_imu[2]);
    Eigen::Vector3d curr_imu_unbias(state.curr_imu[0], state.curr_imu[1], state.curr_imu[2]);
 
    last_imu_unbias[0] -= state.ba[0];
    curr_imu_unbias[0] -= state.ba[0];
    last_imu_unbias[1] -= state.ba[1];
    curr_imu_unbias[1] -= state.ba[1];
    last_imu_unbias[2] -= state.bg;
    curr_imu_unbias[2] -= state.bg;
    
    Eigen::Vector3d imu_unbias_hat = 0.5 * (last_imu_unbias + curr_imu_unbias);
    Eigen::Matrix<double,2,2> sign = Eigen::Matrix2d::Zero();
    sign(0,1) = -1;
    sign(1,0) = 1; 
    Eigen::Matrix<double,2,2> R_dev = sign * R;
	jacobianXji = Eigen::Matrix<double, 5, 8>::Zero();
	jacobianXji.block<2, 2>(0, 0) = Eigen::Matrix<double, 2, 2>::Identity();
	jacobianXji.block<2, 2>(0, 2) = state.dt * Eigen::Matrix<double, 2, 2>::Identity();
	jacobianXji.block<2, 1>(0, 4) = 0.5 * state.dt * state.dt * R_dev * imu_unbias_hat.head<2>();
	jacobianXji.block<2, 2>(0, 5) = 0.5 * state.dt * state.dt * R;

	jacobianXji.block<2, 2>(2, 2) = Eigen::Matrix<double, 2, 2>::Identity();
	jacobianXji.block<2, 1>(2, 4) = 0.5 * state.dt * R_dev * imu_unbias_hat.head<2>();
	jacobianXji.block<2, 2>(2, 5) = state.dt * R;

	jacobianXji(4, 7) = state.dt;

	jacobianXj = Eigen::Matrix<double, 5, 5>::Identity();
}

利用LM求解优化算法，只介绍自己实现的，Ceres-solver用来做对比

void Optimizer::OptimizeLM(State& last_state, State& state, const Eigen::Vector3d &last_velocity, const Eigen::Vector3d &velocity, const Eigen::Vector3d &delta_odometry, const Eigen::Vector3d &last_pose, const Eigen::Vector3d &curr_pose) {
    
    float cost_old = 0.0f;
    bool recompute = true;
    double eps = 10000;
    int runs = 0;
    double lamda =  1e-3;
    double lam_mult = 10;
    double min_dcost = 1e-6;

    Eigen::Matrix<double, 13, 13> Hess = Eigen::Matrix<double, 13, 13>::Zero();
    Eigen::Matrix<double, 13, 1> grad = Eigen::Matrix<double, 13, 1>::Zero();

    Eigen::Matrix<double, 10, 1> residual = Eigen::Matrix<double, 10, 1>::Zero();

    Eigen::Matrix<double, 5, 5> last_pose_covariance = Eigen::Matrix<double, 5, 5>::Identity();
    last_pose_covariance(0,0) = std::pow(0.01,2);
    last_pose_covariance(1,1) = std::pow(0.01,2);
    last_pose_covariance(2,2) = std::pow(0.01,2);
    last_pose_covariance(3,3) = std::pow(0.01,2);
    last_pose_covariance(4,4) = std::pow(0.01,2);
    Eigen::Matrix<double, 5, 5> last_information_matrix = last_pose_covariance.inverse();

    Eigen::Matrix<double, 5, 5> curr_pose_covariance = Eigen::Matrix<double, 5, 5>::Identity();
    curr_pose_covariance(0,0) = std::pow(0.01,2);
    curr_pose_covariance(1,1) = std::pow(0.01,2);
    curr_pose_covariance(2,2) = std::pow(0.01,2);
    curr_pose_covariance(3,3) = std::pow(0.01,2);
    curr_pose_covariance(4,4) = std::pow(0.01,2);
    Eigen::Matrix<double, 5, 5> curr_information_matrix = curr_pose_covariance.inverse();
    
    // cout << "curr_information_matrix == : \n" << curr_information_matrix << endl;

    Eigen::Matrix<double, 10, 10> information_matrix = Eigen::Matrix<double, 10, 10>::Identity();
    information_matrix.block<5,5>(0,0) = last_information_matrix;
    information_matrix.block<5,5>(5,5) = curr_information_matrix;
    Eigen::Matrix<double, 5, 1> residualXi; 
    Eigen::Matrix<double, 5, 8> jacobianXi;

    Eigen::Matrix<double, 5, 1> residualXj; 
    Eigen::Matrix<double, 5, 5> jacobianXj;

    Eigen::Matrix<double, 5, 5> jacobianXij;
    Eigen::Matrix<double, 5, 8> jacobianXji;
    PoseObserve::ComputeResidualAndJacobianPoseGraph(last_state, last_pose, last_velocity.head<2>(), state, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
    cost_old += Chi2(residualXi, last_information_matrix);
    cost_old += Chi2(residualXj, curr_information_matrix);


    while (runs < 100 && lamda < 1e10 && eps > 1e-6) {
        if (recompute) {
            Hess.setZero();
            grad.setZero();
            PoseObserve::ComputeResidualAndJacobianPoseGraph(last_state, last_pose, last_velocity.head<2>(), state, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
            Eigen::Matrix<double,10,13> H = Eigen::Matrix<double,10,13>::Zero();
            H.block<5,8>(0,0) = jacobianXi;
            H.block<5,5>(0,8) = jacobianXij;
            H.block<5,8>(5,0) = jacobianXji;
            H.block<5,5>(5,8) = jacobianXj;
            residual.block<5,1>(0,0) = residualXi;
            residual.block<5,1>(5,0) = residualXj;
            grad.noalias() += -H.transpose() * information_matrix * residual;
            Hess.noalias() += H.transpose() * information_matrix *  H;
        }

        // Solve Levenberg iteration
        Eigen::Matrix<double,13, 13> Hess_l = Hess;
        for (size_t r=0; r < (size_t)Hess.rows(); r++) {
            Hess_l(r,r) *= (1.0 + lamda);
            //std::cout << " lamda : " << lamda << std::endl;
        }

        Eigen::Matrix<double,13,1> dx = Hess_l.colPivHouseholderQr().solve(grad);

        State last_state_tmp = last_state;
        State state_tmp = state;
        update_state(last_state_tmp, state_tmp, dx);
        PoseObserve::ComputeResidualAndJacobianPoseGraph(last_state_tmp, last_pose, last_velocity.head<2>(), state_tmp, curr_pose, velocity.head<2>(), residualXi, jacobianXi, residualXj, jacobianXj, jacobianXij, jacobianXji);
        
        double cost = 0.0;
        cost += Chi2(residualXi, last_information_matrix);
        cost += Chi2(residualXj, curr_information_matrix);
    
        // Check if converged
        if (cost <= cost_old && (cost_old - cost) / cost_old < min_dcost) {
            eps = 0;

            update_state(last_state, state, dx);
            break;
        }

        // If cost is lowered, accept step
        if (cost <= cost_old) {

            recompute = true;
            cost_old = cost;

            update_state(last_state, state, dx);

            runs++;
            lamda = lamda / lam_mult;
            eps = dx.norm();

        } else {
            recompute = false;
            lamda = lamda * lam_mult;
            continue;
        }      
    }
}

上述为经典的LM算法过程，并没有过多技巧，后续逐渐完善

七、总结

本节引入两时刻imu状态的图模型，两时刻的图能够利用上一时刻的bias更新，影响当前帧结果，因此更加稳定些，但直到现在还是没有只观感受直接积分有啥优略势，事实上预积分的影响为两帧间相对变化的约束关系，即x1与x2所连接的边，下一讲来讲解加入积分项后的图优化。

标签

#算法 #SLAM SE2 非线性优化 #人工智能 #计算机视觉