vins-mono视觉惯导对齐

 bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)

    
 {
    
     int all_frame_count = all_image_frame.size(); //所有关键帧数量
    
     int n_state = all_frame_count * 3 + 3 + 1; //所有状态{v_b0_b0, v_b1_b1...v_bn_bn, s, g_c0}的数量
    
  
    
     MatrixXd A{n_state, n_state};
    
     A.setZero();
    
     VectorXd b{n_state};
    
     b.setZero();
    
  
    
     map<double, ImageFrame>::iterator frame_i;
    
     map<double, ImageFrame>::iterator frame_j;
    
     int i = 0;
    
     for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
    
     {
    
     frame_j = next(frame_i);
    
  
    
     MatrixXd tmp_A(6, 10); // 公式中的H
    
     tmp_A.setZero();
    
     VectorXd tmp_b(6); // 公式中的b，也即z_b(k+1)_b(k)
    
     tmp_b.setZero();
    
  
    
     double dt = frame_j->second.pre_integration->sum_dt;
    
  
    
 		// 填入对应的参数
    
     tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
    
     tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();
    
     tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;
    
     tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];
    
     //cout << "delta_p   " << frame_j->second.pre_integration->delta_p.transpose() << endl;
    
     tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
    
     tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
    
     tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();
    
     tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;
    
     //cout << "delta_v   " << frame_j->second.pre_integration->delta_v.transpose() << endl;
    
  
    
     // 此处未将协方差添加进去优化，可能是效果不太好
    
     Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
    
     //cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
    
     //MatrixXd cov_inv = cov.inverse();
    
     cov_inv.setIdentity();
    
  
    
     // 对应公式中的(H)T(H)和(H)T(b)
    
     MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
    
     VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;
    
  
    
     // 多组观测数据时，需要将多个公式的约束关系加起来一起求解
    
     A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
    
     b.segment<6>(i * 3) += r_b.head<6>();
    
  
    
     A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();
    
     b.tail<4>() += r_b.tail<4>();
    
  
    
     A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();
    
     A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();
    
     }
    
     A = A * 1000.0;
    
     b = b * 1000.0;
    
     x = A.ldlt().solve(b);             // 使用LDLT分解法计算状态量
    
     double s = x(n_state - 1) / 100.0; // 提取尺度因子
    
     ROS_DEBUG("estimated scale: %f", s);
    
     g = x.segment<3>(n_state - 4); // 提取重力加速度
    
     ROS_DEBUG_STREAM(" result g     " << g.norm() << " " << g.transpose());
    
     // 如果优化后获得的重力加速度模与设定的重力加速度模差1m/s2，或者尺度因子小于0，则认为视觉惯性对齐失败
    
     if (fabs(g.norm() - G.norm()) > 1.0 || s < 0)
    
     {
    
     return false;
    
     }
    
  
    
     // 注:重力细化过程与线性对齐过程基本一致，只是将待优化的重力向量从三维降为了二维，然后进行优化求解，这里就不展开了．
    
     RefineGravity(all_image_frame, g, x);
    
     s = (x.tail<1>())(0) / 100.0;
    
     (x.tail<1>())(0) = s;
    
     ROS_DEBUG_STREAM(" refine     " << g.norm() << " " << g.transpose());
    
     if (s < 0.0)
    
     return false;
    
     else
    
     return true;
    
 }
![](https://ad.itadn.com/c/weblog/blog-img/images/2025-08-18/dCgPoketfAxBHLZhYzEpFMm6K3D2.png)