VINS-Mono视觉惯性对齐包括三步:
1)陀螺仪偏置矫正
2)速度、重力向量、尺度因子初始化
3)重力修正
c0代表视觉坐标系第0帧,bk代表body坐标系第k帧
(6)
(6)式变为:
(7)
并且已知:
(7)式变为:
所以:
为了能够方便的利用LDLT方法对上式求解,
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;
}
因在上面优化过程中,g也是变量,数值会变化,所以需要修正。
如图,重力矢量大小固定,半球半径即为模长,取切平面上相互垂直的b1、b2方向,便可与重力方向组成三维正交基,实现在三维空间中将重力参数化。
下面就简单了,与第二步相同,用矩阵分解计算。