目录
2.1 滑动窗口(Sliding Window)纯视觉SfM
0 整体框架
1 数据预处理
camera：
1）提取Harris角点，KLT金字塔光流跟踪相邻帧；2）2 维特征点先矫正为不失真的，然后在通过外点剔除后投影到一个单位球面上 ；3）去除异常点：先进行F矩阵测试，通过RANSAC去除异常点；4）关键帧选取：1、当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧（因为视差可以根据平移和旋转共同得到，而纯旋转则导致不能三角化成功，所以这一步需要IMU预积分进行补偿）2、当前帧跟踪到的特征点数量小于阈值视为关键帧；
IMU：
1）两帧k和k+1之间进行位置、速度、姿态（PVQ）预测；2）避免每次姿态优化调整后重复IMU传播，采用预积分算法，计算预积分误差的雅克比矩阵和协方差项。
1.1.1 为何要将特征点投影到单位球面
因为视觉残差的自由度为 2，因此将视觉残差投影到正切平面上， b1,b2 为正切平面上的任意两个正交基。类似于初始化时优化相机坐标系下的g0 ，正交基可以通过施密特正交化来得到。
1）施密特正交化正交化如下
ProjectionFactor::ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j) : pts_i(_pts_i), pts_j(_pts_j)
{
#ifdef UNIT_SPHERE_ERROR
Eigen::Vector3d b1, b2;
Eigen::Vector3d a = pts_j.normalized();
Eigen::Vector3d tmp(0, 0, 1);
if(a == tmp)
tmp << 1, 0, 0;
b1 = (tmp - a * (a.transpose() * tmp)).normalized();
b2 = a.cross(b1);
tangent_base.block<1, 3>(0, 0) = b1.transpose();
tangent_base.block<1, 3>(1, 0) = b2.transpose();
#endif
};2 惯性松耦合初始化
采用松耦合的传感器融合方法得到初始值。首先用SFM进行纯视觉估计滑动窗内所有帧的位姿以及路标点逆深度，然后与IMU预积分对齐，继而恢复对齐尺度s，重力g，imu速度v，和陀螺仪偏置bg。
VINS本文初始化过程中忽视掉了加速度计的bias，因为加速度计与重力耦合，并且重力向量很大，初始化过程动态过程很短，幅度又不大，加速度计偏置很难观测到。
2.1 滑动窗口(Sliding Window)纯视觉SfM
1、选择一个滑动窗，在最后一帧与滑动窗之前帧寻找帧：跟踪到的点数目大于30个的并且视差超过20的，找到后用5点法本质矩阵初始化恢复出R和t。否则，滑动窗内保留最新图像帧，继续等待下一帧。
2、随意设置一个尺度因子，三角化这两帧观测到的所有路标点。再用PnP算法估计滑动窗内所有其余帧的位姿。滑动窗内全局BA重投影误差优化所有帧位姿。
3、假设IMU-Camera外参已知，乘上视觉得到的位姿，转换到IMU坐标系下。
2.2 IMU预积分与视觉结构对齐
2.2.1 估计body到camera的旋转
相邻两个时刻k、k+1之间有 IMU旋转
视觉测量 
。根据手眼标定原理有：
转成四元数形式有：
将多个时刻线性方程叠加起来，并加上鲁棒核函数权重得到
公式2.2.1
其中 
由旋转矩阵和轴角之间的关系 
得到角度误差r的计算为
因此公式2.2.1的求解同样采用SVD分解，即最小奇异值对应的奇异向量。
2.2.2 陀螺仪零偏标定
旋转两种方式：陀螺仪测量值和视觉观测值，二者的误差其实就是陀螺仪偏置bg。
如果外参数qbc已经标定好了，利用旋转约束估计陀螺仪 bias
公式2.2.2
其中B表示所有图像关键帧集合，另有预积分的阶泰勒近似
公式2.2.2为普通的最小二乘问题，构建正定方程 HX=b即可以求解
上述约束方程为：
得：
只考虑虚部得
注意：求解的是零偏的差值 ，且得到了陀螺仪偏置bias的初始校准,需要将陀螺仪偏置bg代入到IMU预积分重新计算预积分。
/**
* @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]);
}
}
之所以 A +=tmp_A.transpose() * tmp_A，其实就是 
，在求解 Ax=b的最小二乘解时，两边同乘以A矩阵的转置得到的AT*A一定是可逆的
推导：
1）公式
性质1
性质2
性质3
因为 
得 
再由性质2得
参考：
VINS Mono solveGyroscopeBias函数解析
2.2.3 速度v、重力g和尺度初始化s
需要估计的变量有 
，其中
表示k时刻body'坐标系的速度，
为重力向量在第0帧相机坐标系下的表示，s表示尺度因子将视觉轨迹拉伸到米制单位。
在世界坐标系w下有
将世界坐标系w换成相机初始时刻坐标系c0有
将
代入上式有
将待估计变量放到方程右边，得
整理得到
公式2.2.3
/**
* @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;
}
2.2.4 优化重力向量
这里de重力加速度矢量，不是我们通常说的绝对重力加速度矢量[0，0，9.81]T,而是相对于c0的重力加速度，值是多少我们暂时还不知道。上式2.2.3 中求解重力向量 
过程中，并没有加入模长限制 
，三维变量实际只有两个自由度，因此采用球面坐标进行参数化，如下图
公式 2.2.4 ，其中w1 w2为待优化变量
将公式 2.2.4代入公式 2.2.3 得
MatrixXd TangentBasis(Vector3d &g0)
{
Vector3d b, c;
Vector3d a = g0.normalized();
Vector3d tmp(0, 0, 1);
if(a == tmp)
tmp << 1, 0, 0;
b = (tmp - a * (a.transpose() * tmp)).normalized();
c = a.cross(b);
MatrixXd bc(3, 2);
bc.block<3, 1>(0, 0) = b;
bc.block<3, 1>(0, 1) = c;
return bc;
}/**
* @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;
}
参考：
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
