VINS-Mono工程笔记（八）：纯视觉SFM及视觉IMU对齐
发布时间
阅读量：
阅读量
1.视觉SFM理论及流程
- 理论及流程
在滑动窗口中共存在11个连续的帧，在确定合适的基准框架时应优先选择一个具有代表性的基准框架，并基于对极几何关系求解该基准框架与最终目标框架之间的相对运动参数。为了确保计算结果的有效性，在选择基准框架时需综合考虑以下两个方面：一方面要求基准框架与最终目标框架之间应具有较大的时间间隔（即两者之间应相距较远），因为当两者过于接近时（时间间隔较小），其间的平移运动幅度会显著减小甚至趋近于零（即平移运动参数为零的情况），这将导致三角化精度降低甚至出现不可行计算；另一方面则要求两者之间的时间间隔不宜过大（即两者间不应过于远离），因为这将导致两者的关联特征点数量减少进而影响到运动参数估计的有效性以及整体定位结果的有效性降低）。因此，在满足足够的特征匹配对数量前提下动态规划算法会选择基准框架与最终目标框架之间尽量拉开较大时间间隔的位置（如图所示），例如在本例中选择了第4号连续 frames 作为基准框架
将第4帧设定为空间参考基准帧，并设置其位姿参数：旋转矩阵\mathbf{I}和平移向量\mathbf{0}。通过极几何关系可求解得到枢纽阵第4帧与最后一帧（编号10）之间的相对位姿\mathbf{R}和平移量\mathbf{t}（其中尺度信息待确定）。由此可知，在已知第4帧的位置后（位置参数：旋转矩阵\mathbf{I}和平移向量\mathbf{0}），可推导出第10帧的位置参数（旋转矩阵\mathbf{R}和平移向量\mathbf{t}）。随后，在已知这些位置参数的基础上进行三角化计算以获得两者的共视点位置（具有尺度因子）。基于光流追踪原理，在获得这两者共视点位置后即可识别到中间各关键点的三维坐标信息。基于此信息应用PnP算法可估算出下一关键帧（编号5）的空间位置参数。采用同样的流程可依次推导出后续各关键帧的空间位置参数直至最后一号框架体空间位置参数得以确定
采用一致的方法进行处理后发现，在枢纽框架（编号为4）与初始框架（编号为0）之间进行位姿及共视点计算时遵循相同的方式。具体而言，在确定了第四框架相对于第一框架的位置之后（因为第四框架的位置设定为单位矩阵和零向量），则计算出的结果即为第一框架的具体姿态信息。随后通过三角测量技术找出两框架之间的共视点坐标，并确保这些特征点在后续的第一至第三框架中都能被检测到。接着利用PnP算法逐步推导出第二至第三框架的具体姿态信息
通过执行上述操作于滑窗区域，即可计算出每帧图像的姿态信息以及三维共视点集合。
- VINS流程
（1）通过加速度标准差判断滑窗内是否具有足够运动。
//1.1.check imu observibility 通过计算标准差来进行判断
{
map<double, ImageFrame>::iterator frame_it;
Vector3d sum_g;
//遍历除了第一帧图像外的所有图像帧
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
double dt = frame_it->second.pre_integration->sum_dt;
//计算每一帧图像对应的加速度
Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
//图像的加速度之累加和
sum_g += tmp_g;
}
Vector3d aver_g;
//计算平均加速度。因为上边计算的是除了第一帧图像之外的其他所有图像帧对应的加速度之和，所以这里要减去1
aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);
double var = 0;
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
double dt = frame_it->second.pre_integration->sum_dt;
//计算获得每一帧的加速度
Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
//加速度减去平均加速度的差值的平方的累加
var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
//cout << "frame g " << tmp_g.transpose() << endl;
}
/** * 简单来说，标准差是一组数值自平均值分散开来的程度的一种测量观念。
* 一个较大的标准差，代表大部分的数值和其平均值之间差异较大；一个较小的标准差，代表这些数值较接近平均值。
* 方差公式：s^2=[(x1-x)^2 +...(xn-x)^2]/n
* 标准差等于方差的算数平方根，标准差公式：s=sqrt(s^2)
*/
var = sqrt(var / ((int)all_image_frame.size() - 1));
//ROS_WARN("IMU variation %f!", var);
//通过加速度标准差判断IMU是否有充分运动，标准差必须大于等于0.25
if(var < 0.25)
{
ROS_INFO("IMU excitation not enouth!");
//return false;
}
}判断加速度的方差保证具有足够的运动。
(2) 遍历每一个 feature，在每个 image frame 中记录 feature 的 ID 以及其他能够观测到该 feature 的帧索引，并记录这些特征在各个 image frame 中的具体坐标位置。这些信息将被保存到 SfMFeature 中。
/** * 1.2.将f_manager.feature中的feature存储到sfm_f中
*/
for (auto &it_per_id : f_manager.feature)
{
int imu_j = it_per_id.start_frame - 1;
SFMFeature tmp_feature;
tmp_feature.state = false;
tmp_feature.id = it_per_id.feature_id;
//遍历每一个能观察到该feature的frame
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
Vector3d pts_j = it_per_frame.point;
tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));
}
sfm_f.push_back(tmp_feature);
}(3) 求解枢纽帧l及与最后一帧的relative_pose
/** * 1.3.位姿求解
*/
Matrix3d relative_R;
Vector3d relative_T;
int l;
//通过求取本质矩阵来求解出位姿
/** * 这里的l表示滑动窗口中第l帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧，
* 会作为参考帧到下面的全局sfm使用，得到的Rt为当前帧到第l帧的坐标系变换Rt，存储在relative_R和relative_T当中
* */
if (!relativePose(relative_R, relative_T, l))
{
ROS_INFO("Not enough features or parallax; Move device around");
return false;
} /** * 这里的第l帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧，
* 会作为参考帧到下面的全局sfm使用，得到的Rt为当前帧到第l帧的坐标系变换Rt
* 该函数判断滑动窗口中第一个到窗口最后一帧对应特征点的平均视差大于30，且内点数目大于12的帧，此时可进行初始化，同时返回当前帧到第l帧的坐标系变换R和T
* */
bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{
// find previous frame which contians enough correspondance and parallex with newest frame
for (int i = 0; i < WINDOW_SIZE; i++)
{
vector<pair<Vector3d, Vector3d>> corres;
//寻找第i帧到窗口最后一帧的对应特征点，存放在corres中
corres = f_manager.getCorresponding(i, WINDOW_SIZE);
//匹配的特征点对要大于20
if (corres.size() > 20)
{
//计算平均视差
double sum_parallax = 0;
double average_parallax;
for (int j = 0; j < int(corres.size()); j++)
{
//第j个对应点在第i帧和最后一帧的(x,y)
Vector2d pts_0(corres[j].first(0), corres[j].first(1));
Vector2d pts_1(corres[j].second(0), corres[j].second(1));
//计算视差
double parallax = (pts_0 - pts_1).norm();
//计算视差的总和
sum_parallax = sum_parallax + parallax;
}
//计算平均视差
average_parallax = 1.0 * sum_parallax / int(corres.size());
//判断是否满足初始化条件：视差>30和内点数满足要求(大于12)
//solveRelativeRT()通过基础矩阵计算当前帧与第l帧之间的R和T,并判断内点数目是否足够
//同时返回窗口最后一帧（当前帧）到第l帧（参考帧）的relative_R，relative_T
if(average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T))
{
//l中存放的是窗口中从最前边开始遍历和最后一帧满足视差和RT估计的第一个帧的id
l = i;
ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l);
return true;
}
}
}
return false;
}从初始帧出发依次扫描（与末帧之间保持较大的平移量以提升三角化的精度），搜索满足共视点数大于30且视差超过30/460的帧；通过极约束求解得到本质矩阵并进一步计算相对姿态。将其标记为关键帧
/** * 根据提供的特征点对计算旋转矩阵和平移向量
*/
bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
if (corres.size() >= 15)
{
vector<cv::Point2f> ll, rr;
for (int i = 0; i < int(corres.size()); i++)
{
ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
}
cv::Mat mask;
/** * 求得本质矩阵
* Mat cv::findFundamentalMat(
* InputArray points1, //第一幅图像中的特征点数组
* InputArray points2, //第二幅图像中的特征点数组
* int method = FM_RANSAC, //计算fundamental matrix的方法，这里使用了cv::FM_RANSAC，说明使用了8点法进行求解的
* CV_FM_7POINT for a 7-point algorithm. N=7
* CV_FM_8POINT for an 8-point algorithm. N≥8
* CV_FM_RANSAC for the RANSAC algorithm. N≥8
* CV_FM_LMEDS for the LMedS algorithm. N≥8
* double ransacReprojThreshold = 3., //点到对极线的最大距离，超过这个值的点将被舍弃
* double confidence = 0.99, //矩阵正确的可信度
* OutputArray mask = noArray() //输出在计算过程中没有被舍弃的点
* )
* 该函数对应链接：https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#gae420abc34eaa03d0c6a67359609d8429
*/
cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
//cameraMatrix初始化为单位矩阵
cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
cv::Mat rot, trans;
//由本质矩阵恢复出位姿
/** * int cv::recoverPose ( InputArray E, //本质矩阵
* InputArray points1, //第一幅图像点的数组
* InputArray points2, //第二幅图像点的数组
* InputArray cameraMatrix, //相机内参
* OutputArray R, //第一帧坐标系到第二帧坐标系的旋转矩阵
* OutputArray t, //第一帧坐标系到第二帧坐标系的平移向量
* InputOutputArray mask = noArray() //在findFundamentalMat()中没有被舍弃的点
* )
*/
int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
//cout << "inlier_cnt " << inlier_cnt << endl;
Eigen::Matrix3d R;
Eigen::Vector3d T;
for (int i = 0; i < 3; i++)
{
T(i) = trans.at<double>(i, 0);
for (int j = 0; j < 3; j++)
R(i, j) = rot.at<double>(i, j);
}
//得到旋转平移
Rotation = R.transpose();
Translation = -R.transpose() * T;
//内点数必须大于12
if(inlier_cnt > 12)
return true;
else
return false;
}
return false;
}通过调用cv::findFundamentalMat()函数计算出基本矩阵，并因输入中的匹配点已排除了相机内参数的影响而无需额外处理即可直接获得基本矩阵。基本矩阵具有五个自由度（其中t代表相对方向无尺度），因此理论上仅需五对对应点即可确定基本矩阵；然而，在实际应用中可能会出现一些误匹配情况；因此我们采用了RANSEC算法来进行去噪处理，并要求至少需要 fifteen correspondence pairs才能进行可靠估计；最后通过recoverPose函数分解姿态信息并判断有效的内点数量是否超过 twelve以确保结果的有效性
(4) 三角化和PNP求解地图点和每帧相机位姿
GlobalSFM sfm;
/** * 1.4.三角化特征点，对滑窗每一帧求解sfm问题
*/
if(!sfm.construct(frame_count + 1, Q, T, l,
relative_R, relative_T,
sfm_f, sfm_tracked_points))
{
ROS_DEBUG("global SFM failed!");
marginalization_flag = MARGIN_OLD;
return false;
} /** * 对窗口中每个图像帧求解sfm问题，得到所有图像帧相对于参考帧的旋转四元数Q、平移向量T和特征点坐标sfm_tracked_points
* 参数frame_num的值为frame_count + 1
* 传入的参数l就是参考帧的index
* */
bool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l,
const Matrix3d relative_R, const Vector3d relative_T,
vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points)
{
feature_num = sfm_f.size();
//cout << "set 0 and " << l << " as known " << endl;
// have relative_r relative_t
// intial two view
//q为四元数数组，大小为frame_count+1
q[l].w() = 1;
q[l].x() = 0;
q[l].y() = 0;
q[l].z() = 0;
//T为平移量数组，大小为frame_count+1
T[l].setZero();
//往q中存入最新帧的旋转，该旋转等于第l帧的旋转和相对旋转的四元数乘积
q[frame_num - 1] = q[l] * Quaterniond(relative_R);
T[frame_num - 1] = relative_T;
//cout << "init q_l " << q[l].w() << " " << q[l].vec().transpose() << endl;
//cout << "init t_l " << T[l].transpose() << endl;
//rotate to cam frame
Matrix3d c_Rotation[frame_num];
Vector3d c_Translation[frame_num];
Quaterniond c_Quat[frame_num];
double c_rotation[frame_num][4];
double c_translation[frame_num][3];
Eigen::Matrix<double, 3, 4> Pose[frame_num];
c_Quat[l] = q[l].inverse();//c_Quat[l]中存放q[l]的逆
c_Rotation[l] = c_Quat[l].toRotationMatrix();//四元数转为旋转矩阵
c_Translation[l] = -1 * (c_Rotation[l] * T[l]);
/** * matrix.block(i,j, p, q) : 表示返回从矩阵(i, j)开始，每行取p个元素，每列取q个元素所组成的临时新矩阵对象，原矩阵的元素不变；
* matrix.block<p,q>(i, j) :<p, q>可理解为一个p行q列的子矩阵，该定义表示从原矩阵中第(i, j)开始，获取一个p行q列的子矩阵，
* 返回该子矩阵组成的临时矩阵对象，原矩阵的元素不变；
*/
//将第l帧的旋转和平移量存入到Pose当中
Pose[l].block<3, 3>(0, 0) = c_Rotation[l];
Pose[l].block<3, 1>(0, 3) = c_Translation[l];
c_Quat[frame_num - 1] = q[frame_num - 1].inverse();
c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();
c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);
Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];
Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];
//至此，计算出了滑动窗口中第l帧的位姿Pose[l]和最新帧的位姿Pose[frame_num-1]，下面就是三角化处理
//1: trangulate between l ----- frame_num - 1
//2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1;
for (int i = l; i < frame_num - 1 ; i++)//对于参考帧和当前帧之间的某一帧，三角化该帧和当前帧的路标点
{
// solve pnp
if (i > l)//不是参考帧
{
Matrix3d R_initial = c_Rotation[i - 1];
Vector3d P_initial = c_Translation[i - 1];
if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
return false;
c_Rotation[i] = R_initial;
c_Translation[i] = P_initial;
c_Quat[i] = c_Rotation[i];
Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
Pose[i].block<3, 1>(0, 3) = c_Translation[i];
}
// triangulate point based on the solve pnp result
triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
}
//3: triangulate l-----l+1 l+2 ... frame_num -2
for (int i = l + 1; i < frame_num - 1; i++)//对于参考帧和当前帧之间的某一帧，三角化参考帧和该帧的路标点
triangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);
//4: solve pnp l-1; triangulate l-1 ----- l
// l-2 l-2 ----- l
for (int i = l - 1; i >= 0; i--)//对于第一帧和参考帧之间的某一帧,先用PnP求解该帧位姿,然后三角化该帧到参考帧的路标点
{
//solve pnp
Matrix3d R_initial = c_Rotation[i + 1];
Vector3d P_initial = c_Translation[i + 1];
if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
return false;
c_Rotation[i] = R_initial;
c_Translation[i] = P_initial;
c_Quat[i] = c_Rotation[i];
Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
Pose[i].block<3, 1>(0, 3) = c_Translation[i];
//triangulate
triangulateTwoFrames(i, Pose[i], l, Pose[l], sfm_f);
}
//5: triangulate all other points
for (int j = 0; j < feature_num; j++)//三角化其他未恢复的路标点
{
if (sfm_f[j].state == true)
continue;
if ((int)sfm_f[j].observation.size() >= 2)
{
Vector2d point0, point1;
int frame_0 = sfm_f[j].observation[0].first;
point0 = sfm_f[j].observation[0].second;
int frame_1 = sfm_f[j].observation.back().first;
point1 = sfm_f[j].observation.back().second;
Vector3d point_3d;
triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, point_3d);
sfm_f[j].state = true;
sfm_f[j].position[0] = point_3d(0);
sfm_f[j].position[1] = point_3d(1);
sfm_f[j].position[2] = point_3d(2);
//cout << "trangulated : " << frame_0 << " " << frame_1 << " 3d point : " << j << " " << point_3d.transpose() << endl;
}
}
/*
for (int i = 0; i < frame_num; i++)
{
q[i] = c_Rotation[i].transpose();
cout << "solvePnP q" << " i " << i <<" " <<q[i].w() << " " << q[i].vec().transpose() << endl;
}
for (int i = 0; i < frame_num; i++)
{
Vector3d t_tmp;
t_tmp = -1 * (q[i] * c_Translation[i]);
cout << "solvePnP t" << " i " << i <<" " << t_tmp.x() <<" "<< t_tmp.y() <<" "<< t_tmp.z() << endl;
}
*/
//full BA
ceres::Problem problem;
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
//cout << " begin full BA " << endl;
for (int i = 0; i < frame_num; i++)
{
//double array for ceres
c_translation[i][0] = c_Translation[i].x();
c_translation[i][1] = c_Translation[i].y();
c_translation[i][2] = c_Translation[i].z();
c_rotation[i][0] = c_Quat[i].w();
c_rotation[i][1] = c_Quat[i].x();
c_rotation[i][2] = c_Quat[i].y();
c_rotation[i][3] = c_Quat[i].z();
problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
problem.AddParameterBlock(c_translation[i], 3);
if (i == l)
{
problem.SetParameterBlockConstant(c_rotation[i]);
}
if (i == l || i == frame_num - 1)
{
problem.SetParameterBlockConstant(c_translation[i]);
}
}
for (int i = 0; i < feature_num; i++)
{
if (sfm_f[i].state != true)
continue;
for (int j = 0; j < int(sfm_f[i].observation.size()); j++)
{
int l = sfm_f[i].observation[j].first;
ceres::CostFunction* cost_function = ReprojectionError3D::Create(
sfm_f[i].observation[j].second.x(),
sfm_f[i].observation[j].second.y());
problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l],
sfm_f[i].position);
}
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.minimizer_progress_to_stdout = true;
options.max_solver_time_in_seconds = 0.2;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03)
{
//cout << "vision only BA converge" << endl;
}
else
{
//cout << "vision only BA not converge " << endl;
return false;
}
for (int i = 0; i < frame_num; i++)
{
q[i].w() = c_rotation[i][0];
q[i].x() = c_rotation[i][1];
q[i].y() = c_rotation[i][2];
q[i].z() = c_rotation[i][3];
q[i] = q[i].inverse();
//cout << "final q" << " i " << i <<" " <<q[i].w() << " " << q[i].vec().transpose() << endl;
}
for (int i = 0; i < frame_num; i++)
{
T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));
//cout << "final t" << " i " << i <<" " << T[i](0) <<" "<< T[i](1) <<" "<< T[i](2) << endl;
}
for (int i = 0; i < (int)sfm_f.size(); i++)
{
if(sfm_f[i].state)
sfm_tracked_points[sfm_f[i].id] = Vector3d(sfm_f[i].position[0], sfm_f[i].position[1], sfm_f[i].position[2]);
}
return true;
}step1：三角化枢纽帧和最后一帧(l -> frame-1)，求解特征点。
step2: 对(l+1 -> frame-2)帧基于l帧初始位姿(I)进行pnp求解。
step3: 对(l+1 -> frame-2)帧和最后一帧三角化。
step4: 对(l-1 -> 0)帧基于l帧初始位姿(I)进行pnp求解并和l帧进行三角化，
step5: 对于还没有处理过的特征点进行三角化。
step6: 构建全局BA框架，在各帧的姿态信息下实现反投影误差的最小化目标。具体而言，在此过程中我们采用了不优化枢纽帧旋转参数及最后一帧平移量的方式进行处理
2.视觉IMU对齐
1）理论
- 流程
陀螺仪零偏差估计solveGyroscopeBias()结合视觉帧间角度估计（上文SFM中获得）与IMU角度积分值（对应图像帧）得出。
只考虑虚部，从而得到AX=b的问题。其中J表示姿态对bg的雅可比。
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]);
}
}最后最小二乘求解得到delta_bg，加到bg上。
(2)尺度，重力加速度，速度计算LinearAlignment()
通过比较加速度积分值与相机SFM所得的位移信息,经过计算得出尺度,重力加速度等信息
约束公式如下：
通过整理上式，将待求解量提取出来，可将上式化为以下形式：
其中：
从而可构成最小二乘问题：
如此问题变为A x = b Ax=bAx=b的问题，使用x = A.ldlt().solve(b)进行求解。
(3)重力向量优化
由于重力矢量的模大小是已知的，因此剩下两个自由度。在半径为
在半球面上的切面空间内采用两个相互垂直的向量对重力场进行参数化描述
此时重力向量表示为：
同样的方法进行求解A x = b 问题。
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;
}3.对齐后世界系位姿更新
当从视觉SFM中提取的位姿数据与IMU预积分结果进行对齐处理之后
bool Estimator::visualInitialAlign()
{
TicToc t_g;
VectorXd x;
//solve scale
bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
if(!result)
{
ROS_DEBUG("solve g failed!");
return false;
}
// change state
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;
}
VectorXd dep = f_manager.getDepthVector();
for (int i = 0; i < dep.size(); i++)
dep[i] = -1;
f_manager.clearDepth(dep);
//triangulat on cam pose , no tic
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);
for (int i = 0; i <= WINDOW_SIZE; i++)
{
pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}
for (int i = frame_count; i >= 0; i--)
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++;
Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
}
}
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;
}
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;
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];
}
ROS_DEBUG_STREAM("g0 " << g.transpose());
ROS_DEBUG_STREAM("my R0 " << Utility::R2ypr(Rs[0]).transpose());
return true;
}基于获得的重力加速度和尺度等关键参数之后，在此基础上再次进行三角化计算，并完成积分运算；同时，在f_manager中调用triangulate过程，并对pre_integrations数组中的第i个元素执行反向传播计算。
使用尺度信息恢复视觉偏移及特征点深度；
将世界坐标系的z轴与重力加速度方向对齐，并同时将特征点转换至该世界坐标系中
下一章 后端优化~~~~~~~~~
全部评论 (0)
还没有任何评论哟~