VINS-MONO代码解读---初始化2：视觉惯性松耦合初始化 visualIntialAlign（）
发布时间
阅读量：
阅读量
visualIntialAlign（）函数视觉惯性联合初始化
本文围绕探讨视觉模块与惯性测量单元（IMU）之间的关系，并详细分析如何协调与统一这两者以确保系统的初始化过程顺利完成。
目录
理论：视觉IMU对齐流程
其中，步骤1的在线Cam到IMU的外参标定qbc 参考之前的博客。
1、陀螺仪零偏bg标定
2、优化 速度v、重力g和尺度初始化s。我们得到了陀螺仪偏置bias的初始校准,需要将陀螺仪偏置bg代入到IMU预积分重新计算预积分
3、重力矢量修正
bash代码流程
bool Estimator::visualInitialAlign()
主要过程为：
VisualIMUAlignment（）函数计算陀螺仪偏置bg，尺度s，重力加速度g和速度v
2.f_manager.triangulate（）计算特征点深度estimated_depth
3.repropagate（）陀螺仪的偏置bgs改变，重新计算预积分
4.将Ps、Vs、depth进行更新
5.将重力旋转到Z轴，将Ps、Vs、Rs从相机参考坐标系c0旋转到世界坐标系w。
bash
1、计算陀螺仪偏置bg，尺度s，重力加速度g和速度v
bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
if(!result)
{
ROS_DEBUG("solve g failed!");
return false;
}
cpp这里的VisualIMUAlignment（）函数最重要，我们会在下一讲详细讲解。
这里先熟悉基本流程。
2、得到所有图像帧的位姿Ps、Rs，并将其置为关键帧
for (int i = 0; i <= frame_count; i++)
{
Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;
Vector3d Pi = all_image_frame[Headers[i].stamp.toSec()].T;
Ps[i] = Pi;
Rs[i] = Ri;
all_image_frame[Headers[i].stamp.toSec()].is_key_frame = true;
}
cpp3、重新计算特征点的深度depth
VectorXd dep = f_manager.getDepthVector();
for (int i = 0; i < dep.size(); i++)
dep[i] = -1;
f_manager.clearDepth(dep);
Vector3d TIC_TMP[NUM_OF_CAM];
for(int i = 0; i < NUM_OF_CAM; i++)
TIC_TMP[i].setZero();
ric[0] = RIC[0];
f_manager.setRic(ric);
f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));
// 尺度先假设
double s = (x.tail<1>())(0);
cpp
4、陀螺仪的偏置bgs改变，重新计算预积分
for (int i = 0; i <= WINDOW_SIZE; i++)
{
pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}
cpp这里最要的repropagate（）函数很重要。
5. 将Ps, Vs, depth重新计算
5.1 主要任务是将物体的姿态从相机固定的初始坐标系c0归位至惯性测量单元（IMU）所在的参考坐标系。
// 5、将Ps、Vs、depth进行更新
for (int i = frame_count; i >= 0; i--)
// 5.1 Ps转变为第i帧imu坐标系到第0帧imu坐标系的变换
// 之前相机第l帧为参考系，转换到IMU bo为基准坐标系
Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);
int kv = -1;
map<double, ImageFrame>::iterator frame_i;
for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)
{
if(frame_i->second.is_key_frame)
{
kv++;
//5.2 Vs为优化得到的速度
Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
}
}
// 5.3 逆深度depth更新
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
it_per_id.estimated_depth *= s;
}
cpp
6、所有变量从参考坐标系c0到世界坐标系。
// 6 通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系c0之间的旋转矩阵rot_diff
Matrix3d R0 = Utility::g2R(g);
double yaw = Utility::R2ypr(R0 * Rs[0]).x();
R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
g = R0 * g;
//Matrix3d rot_diff = R0 * Rs[0].transpose();
Matrix3d rot_diff = R0;
// 7/所有变量从参考坐标系c0旋转到世界坐标系w
for (int i = 0; i <= frame_count; i++)
{
Ps[i] = rot_diff * Ps[i];
Rs[i] = rot_diff * Rs[i];
Vs[i] = rot_diff * Vs[i];
}
cpp
VisualIMUAlignment（）
本节将重点阐述Estimator模块中的视觉惯性联合初始化模块 visualIntialAlign（）函数中的第一步 VisualIMUAlignment（）函数，在上一节中对该流程进行了简要概述的基础上，本节将对其第一部分 VisualIMUAlignment（）函数展开深入讨论。
理论知识： 本节将重点阐述初始化阶段中的视觉惯性配准过程及其在陀螺仪零偏差校正中的应用。文中将详细讨论重力加速度g在计算中的应用以及每帧的速度v和尺度s的具体处理方法，并深入分析相机与IMU之间的相对位置关系确定。
主要流程为：
利用旋转约束估计陀螺仪偏置bg
利用平移约束估计重力加速度g、每帧速度v、尺度s
对重力向量g^{c0}进一步优化
bash视觉惯性校准：IMU预积分与视觉结构对齐
VINS-MONO代码解读—在线Cam到IMU的外参标定InitialEXRotation类求解出来后，则可基于旋转约束条件来估计陀螺仪零偏值。
VINS-Mono代码解读—在线Cam到IMU的外参标定InitialEXRotation类
两种旋转测速方法：采用IMU测速数据与视觉测速信息作为输入信号，在此基础之上分析其误差特性时发现其误差本质上源于IMU的偏置参数bg
目标函数用于衡量视觉手段下两帧之间旋转的一致性与IMU预积分结果的差异程度
基于陀螺仪bias噪声的影响因素分析下,我们通过校准流程获得陀螺仪偏置bias的初始值,在进行IMU预积分时需对陀罗仪偏置bg参数进行重新计算以提高系统的整体精度
2、速度v、重力g和尺度初始化s
优化变量：速度、重力向量和尺度
目标函数：相邻两帧IMU预积分增量与预测值之间的平移和速度差异。基于矩阵形式HX = B的方法，并利用cholesky分解进行求解。
3、重力矢量修正
重力向量的大小是已知的，加入了模长限制
，这导致三维重力向量只剩2个自由度。
主要做的是优化方向，一个二维向量。
在其切线空间上用两个变量重新参数化重力，采用球面坐标进行参数化：
4、将相机坐标系对齐世界坐标系
已知量：
1）上一步得到了重力向量g^{c0}在相机初始时刻C_{0}系下的大小,
2）重力向量在世界坐标系下的绝对向量为g=[0,0,9.81]
bash
最后一步
视觉惯性对齐算法的实现存在于vins_estimator/src/initial/initial_aligment.cpp/.h
该函数主要被调用两个子函数来完成功能：一个是陀螺仪偏置的标定与计算重力加速度；另一个是计算速度。
//视觉和IMU对齐
bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
{
solveGyroscopeBias(all_image_frame, Bgs);//计算陀螺仪偏置
if(LinearAlignment(all_image_frame, g, x))//计算尺度，重力加速度和速度
return true;
else
return false;
}
cpp
ImageFrame图像帧类主要用于位于initial_alignment.h文件中；该头文件主要包括两个主要内容：ImageFrame类及其VisualIMUAlignment（）函数。
ImageFrame图像帧为特征点、时间戳、相机位姿、预积分对象、是否关键帧。
陀螺仪偏置标定solveGyroscopeBias（）函数
基于SFM计算所得的旋转矩阵与通过IMU预积分获得的旋转量q并采用Ax=B的方法进行分解以获得最小误差的目标
注意获得新的Bias后存储到Bgs[]中，并对应地重新计算一次其预积分(repropagate)
/** * @brief 陀螺仪偏置校正
* @optional 根据视觉SFM的结果来校正陀螺仪Bias -> Paper V-B-1
* 主要是将相邻帧之间SFM求解出来的旋转矩阵与IMU预积分的旋转量对齐
* 注意得到了新的Bias后对应的预积分需要repropagate
* @param[in] all_image_frame所有图像帧构成的map,图像帧保存了位姿、预积分量和关于角点的信息
* @param[out] Bgs 陀螺仪偏置
* @return void
*/
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
Matrix3d A;
Vector3d b;
Vector3d delta_bg;
A.setZero();
b.setZero();
map<double, ImageFrame>::iterator frame_i;
map<double, ImageFrame>::iterator frame_j;
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
{
frame_j = next(frame_i);
MatrixXd tmp_A(3, 3);
tmp_A.setZero();
VectorXd tmp_b(3);
tmp_b.setZero();
//R_ij = (R^c0_bk)^-1 * (R^c0_bk+1) 转换为四元数 q_ij = (q^c0_bk)^-1 * (q^c0_bk+1)
Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
//tmp_A = J_j_bw
tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
//tmp_b = 2 * (r^bk_bk+1)^-1 * (q^c0_bk)^-1 * (q^c0_bk+1)
// = 2 * (r^bk_bk+1)^-1 * q_ij
tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
//tmp_A * delta_bg = tmp_b
A += tmp_A.transpose() * tmp_A;
b += tmp_A.transpose() * tmp_b;
}
// LDLT方法
delta_bg = A.ldlt().solve(b);
ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());
for (int i = 0; i <= WINDOW_SIZE; i++)
Bgs[i] += delta_bg;
// 得到了新的Bias后对应的预积分需要repropagate
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
{
frame_j = next(frame_i);
frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
}
}
cpp
计算尺度、重力、速度 LinearAlignment（）函数
主要就是对于Ax=b的填充
/** * @brief 计算尺度，重力加速度和速度
* @optional 速度、重力向量和尺度初始化Paper -> V-B-2
* 相邻帧之间的位置和速度与IMU预积分出来的位置和速度对齐，求解最小二乘
* 重力细化 -> Paper V-B-3
* @param[in] all_image_frame 所有图像帧构成的map,图像帧保存了位姿，预积分量和关于角点的信息
* @param[out] g 重力加速度
* @param[out] x 待优化变量，窗口中每帧的速度V[0:n]、重力g、尺度s
* @return void
*/
bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
int all_frame_count = all_image_frame.size();
//优化量x的总维度
int n_state = all_frame_count * 3 + 3 + 1;
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);
tmp_A.setZero();
VectorXd tmp_b(6);
tmp_b.setZero();
double dt = frame_j->second.pre_integration->sum_dt;
// tmp_A(6,10) = H^bk_bk+1 = [-I*dt 0 (R^bk_c0)*dt*dt/2 (R^bk_c0)*((p^c0_ck+1)-(p^c0_ck)) ]
// [ -I (R^bk_c0)*(R^c0_bk+1) (R^bk_c0)*dt 0 ]
// tmp_b(6,1 ) = z^bk_bk+1 = [ (a^bk_bk+1)+(R^bk_c0)*(R^c0_bk+1)*p^b_c-p^b_c , (b^bk_bk+1)]^T
// tmp_A * x = tmp_b 求解最小二乘问题
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();
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);
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());
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;
}
cpp
重力细化RefineGravity（）函数
/** * @brief 重力矢量细化
* @optional 重力细化，在其切线空间上用两个变量重新参数化重力 -> Paper V-B-3
g^ = ||g|| * (g^-) + w1b1 + w2b2
* @param[in] all_image_frame 所有图像帧构成的map,图像帧保存了位姿，预积分量和关于角点的信息
* @param[out] g 重力加速度
* @param[out] x 待优化变量，窗口中每帧的速度V[0:n]、二自由度重力参数w[w1,w2]^T、尺度s
* @return void
*/
void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
//g0 = (g^-)*||g||
Vector3d g0 = g.normalized() * G.norm();
Vector3d lx, ly;
//VectorXd x;
int all_frame_count = all_image_frame.size();
int n_state = all_frame_count * 3 + 2 + 1;
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;
for(int k = 0; k < 4; k++)//迭代4次
{
//lxly = b = [b1,b2]
MatrixXd lxly(3, 2);
lxly = TangentBasis(g0);
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, 9);
tmp_A.setZero();
VectorXd tmp_b(6);
tmp_b.setZero();
double dt = frame_j->second.pre_integration->sum_dt;
// tmp_A(6,9) = [-I*dt 0 (R^bk_c0)*dt*dt*b/2 (R^bk_c0)*((p^c0_ck+1)-(p^c0_ck)) ]
// [ -I (R^bk_c0)*(R^c0_bk+1) (R^bk_c0)*dt*b 0 ]
// tmp_b(6,1) = [ (a^bk_bk+1)+(R^bk_c0)*(R^c0_bk+1)*p^b_c-p^b_c - (R^bk_c0)*dt*dt*||g||*(g^-)/2 , (b^bk_bk+1)-(R^bk_c0)dt*||g||*(g^-)]^T
// tmp_A * x = tmp_b 求解最小二乘问题
tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly;
tmp_A.block<3, 1>(0, 8) = 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] - frame_i->second.R.transpose() * dt * dt / 2 * g0;
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, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly;
tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0;
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();
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<3, 3>() += r_A.bottomRightCorner<3, 3>();
b.tail<3>() += r_b.tail<3>();
A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();
A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();
}
A = A * 1000.0;
b = b * 1000.0;
x = A.ldlt().solve(b);
//dg = [w1,w2]^T
VectorXd dg = x.segment<2>(n_state - 3);
g0 = (g0 + lxly * dg).normalized() * G.norm();
//double s = x(n_state - 1);
}
g = g0;
}
cpp
全部评论 (0)
还没有任何评论哟~