目录
VIO 相关知识回顾
IMU 传感器模型:
(1)
IMU 预积分:
将一段时间内的 IMU 数据直接积分起来就能得到两时刻 i,j 之间关于 IMU 的测量约束,即
预积分量 :
(2)
丢失了尺度信息
用 Typora 画的,字体有点小了
VINS 鲁棒初始化
视觉和 IMU 之间的联系
相机坐标系
为世界坐标系,利用外参数
构建等式:
(3)
其中,
为尺度因子,
表示非米制单位的轨迹。等式(3)等价:
(4)
式子中的
这样理解:camera 和 imu body 之间有一个
轨迹的变换,这个变换是在 world 坐标系下,那
就是从 world 坐标系(
坐标系)变换到现在要计算轨迹的
坐标系下。(4)第一个中第一个式子也可以写成这样:
。
估计流程
① 旋转外参数
未知,则先估计旋转外参数.
② 利用旋转约束估计陀螺仪 bias.
高斯白噪声 n 每个时刻不确定,无法估计,bias 认为在短时间内是不变的,可以估计出后减去得到更准确的 omega
③ 利用平移约束估计重力方向,速度,以及尺度初始值.
④ 对重力向量
进一步优化.
⑤ 求解世界坐标系
和初始相机坐标系
之间的旋转矩阵
,并将轨迹对齐到世界坐标系(可以用
也就是相机坐标系
的重力加速度,与
对比,求算出之间的旋转关系).
论文:
Zhenfei, Yang, Shaojie,等. Monocular Visual–Inertial State Estimation With Online Initialization and Camera–IMU Extrinsic Calibration[J]. IEEE Transactions on Automation Science & Engineering, 2017.
2. Qin T , Shen S . Robust initialization of monocular visual-inertial estimation on aerial robots[C]// IEEE/RSJ International Conference on Intelligent Robots & Systems. IEEE, 2017:4225-4232.
利用旋转约束估计外参数旋转
相邻两时刻
之间有:IMU 旋转积分
,视觉测量
。则有:
(5)
上式可写成:
(6)
其中,
表示 left and right quaternion multiplication。
将多个时刻线性方程累计起来,并加上
鲁棒核权重 得到:
(7)
其中:
(8)目的就是把残差大的权重设置的小一点
由旋转矩阵和轴角之间的关系
,能得到角度误差
的计算为:
(9) 这里的acos就是arccos,如果求迹的括号内是无误差的,那么tr()内会是一个单位阵
公式 (7) 最小二乘解仍使用 SVD 分解,取最小奇异值对应的奇异向量。
这里简单解释一下为什么取
的最小奇异值对应的奇异向量,能使得最小二乘问题
最接近零:
可以假设
是任意的奇异向量,
是系数,这样一对基向量可以表示在
的空间下所有向量。
就可以展开为:
公式是手推的,有问题欢迎提出来!
当
能使问题最小。最小奇异值对应的奇异向量是
分解后,
的最右边一列。
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
frame_count++;
Rc.push_back(solveRelativeR(corres));
Rimu.push_back(delta_q_imu.toRotationMatrix());
Rc_g.push_back(ric.inverse() * delta_q_imu * ric);
Eigen::MatrixXd A(frame_count * 4, 4);
A.setZero();
int sum_ok = 0;
for (int i = 1; i <= frame_count; i++)
{
Quaterniond r1(Rc[i]);
Quaterniond r2(Rc_g[i]);
double angular_distance = 180 / M_PI * r1.angularDistance(r2);
//ROS_DEBUG("%d %f", i, angular_distance);
double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
++sum_ok;
Matrix4d L, R;
double w = Quaterniond(Rc[i]).w();
Vector3d q = Quaterniond(Rc[i]).vec();
L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
L.block<3, 1>(0, 3) = q;
L.block<1, 3>(3, 0) = -q.transpose();
L(3, 3) = w;
Quaterniond R_ij(Rimu[i]);
w = R_ij.w();
q = R_ij.vec();
R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
R.block<3, 1>(0, 3) = q;
R.block<1, 3>(3, 0) = -q.transpose();
R(3, 3) = w;
A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
}
JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
Matrix<double, 4, 1> x = svd.matrixV().col(3);
Quaterniond estimated_R(x);
ric = estimated_R.toRotationMatrix().inverse();
//cout << svd.singularValues().transpose() << endl;
//cout << ric << endl;
Vector3d ric_cov;
ric_cov = svd.singularValues().tail<3>();
if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
{
calib_ric_result = ric;
return true;
}
else
return false;
}
基于旋转约束的 Gyroscope Bias
如果外参数
已经标定好,利用旋转约束,可估计陀螺仪 bias:
(10)
是相机测得的 k 到 k+1 的变换,
是 imu 测得的,这里用视觉修正 imu。
其中,
表示取虚部,对于这个问题来说,2倍的就是角度
,因为
。
表示所有的图像关键帧集合,另有预积分的一阶泰勒近似:
(11) 四元数用imu测出的角速度ω更新,ω中有bias,估计出能使更新更准确
公式(10)是普通的最小二乘问题,求其雅可比矩阵,构建正定方程
即可求解。前面那个方程是超正定的,即方程个数超过更新变量的个数。
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();
Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
A += tmp_A.transpose() * tmp_A;
b += tmp_A.transpose() * tmp_b;
}
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;
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]);
}
}
初始化速度、重力和尺度因子
需要估计的变量:
(12)
其中,
表示 k 时刻 body 坐标系的速度在 body 坐标系下的表示。
为重力向量在第 0 帧相机坐标系下的表示。
表示 scale 就是尺度因子,将视觉轨迹拉伸到米制单位,统一 imu 和视觉单位。
预积分量约束:
世界坐标系
下有:
(13)
简单来说:公式就是就是把影响
和
的除预积分外的所有因素都减去了,其实就是等于预积分量。
把世界坐标系
换成相机初始时刻坐标系
有:
(14) 回顾:在更新速度的时候,需要每个IMU时刻的世界坐标系下的旋转,在更新平移时,需要每个IMU时刻世界坐标系下的速度和旋转。预积分量与世界坐标系下的旋转和速度是解偶的,只需要通过IMU的读数和前一时刻的预积分量就可以更新当前的预积分量
与↓
(3)
结合,有:
(15)
而不是
的原因是
的这个负号。
将待估计的变量放在方程右边,有:
(16) 矩阵括号内,框选的是与初始化速度、尺度因子、重力向量相关的项
(17)
由于预积分的计算中 imu 测得的角速度和加速度并不是完全满足公式的,存在 bias 和高斯白噪声,可以用协方差矩阵的传递,最终得到公式估计值和真实值的偏差
,构建最小二乘问题求解,即
何值时,能使得偏差最小,估计值最接近真实值:
(18)
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;
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.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;
}
优化重力向量
重力向量在三维空间下是三自由度的,但是加入
的限定,实际上只剩下
两个自由度 。多一个限制就少一个自由度这个很好理解吧,和机械设计中低副限制两个自由度、高副限制一个自由度是一样的。
重力向量参数化:
一种简单的参数化方法: 一个三维向量,模长一定即
,和球面方程相同,可以通过求下图中
与球面相交于一点,球心到这一点即重力向量方向。这里的
就是两个自由度的参数化。
Tangent space 切向空间
VINS-Mono中参数化方法: 三维向量自由度为 2,采用球面坐标参数化:
(19)
可以构建一个新的坐标系,表示空间内任意一个三维向量。
是上一个步骤中估计出的重力向量,其中,
是待优化变量。
(20) 这里就是用了叉乘的性质
公式 (19) 带入公式 (16),待优化变量变成:
(21)
观测方程 (16) 也变成了:
(22) 可以看到把上部分求解出的 g 减去了,更新的变量里仅含对 g 的优化值 w
采用最小二乘对
重新优化。
相机坐标系对齐世界坐标系:
(23) u使用叉乘的性质,是垂直于两个坐标系g的单位向量(轴)。θ利用叉乘和点乘得到的tanθ得到,旋转矩阵可以用轴角式得到AngleAxis-&amp;amp;amp;amp;amp;gt;RotationMatrix。这里有一个细节:w 到 c0 的旋转矩阵,θ是由c0 到w计算的,可以简单想象,g垂直向下,c0相对w沿x轴旋转θ&amp;amp;amp;amp;amp;#39;,g在c0下的变换是不是反向的?
把相机坐标系下的变量旋转到世界坐标系下。
把相机平移和特征点尺度恢复到米制单位。
其他初始化方案:
VINS 系统
VINS 系统优化的状态变量为:
(24)
最小化滑动窗口中的残差项估计系统的状态变量:
先验指的是滑动窗口算法中 marg 掉的变量传递的信息矩阵
鲁棒核函数
只用来处理 outlier。
评论(0)
您还未登录,请登录后发表或查看评论