1.视觉SFM理论及流程
1) 理论及流程
假设滑窗中一共有11帧，首先需要选取一个枢纽帧，利用枢纽帧和最后一帧通过对极约束求出这两帧之间的相对位姿。对枢纽帧的要求是：一方面要求枢纽帧离最后一帧尽可能远，因为离最后一帧比较近时，二者之间的平移比较小，使得三角化精度较差，甚至会无法进行对极约束（平移为零的情况）。另一方面，要求枢纽帧与最后一帧之间不能距离太远，否则会使得二者之间关联的特征点比较少，导致求解出的位姿可信度不高，并且无法三角化出尽可能多的点。因此，在保证匹配点足够多的情况下，枢纽帧离最后一帧应该尽可能远，如下图中选取第4帧作为枢纽帧。
将枢纽帧设为参考帧，其位姿设置：旋转为单位阵I II，平移为0。通过对极几何求出枢纽阵(4)和最后一帧(10)之间的相对位姿R和t（尺度未知）。由于第4帧的位姿为( I , 0 ) (I,0)(I,0)，那么第10帧的位姿就是二者之间的相对位姿( R , t ) (R,t)(R,t)。然后，在已知位姿之后，通过三角化求出这两帧之间的共视点（带尺度的）。由于光流追踪的性质，求出的第4帧与第10帧之间的共视点也会被中间的第5—9帧所看到，那么在已知这些共视点（3D）和第4帧的关键点（2D）之后，通过PnP的方法（3D-2D）求解出第5帧的位姿。按照同样的方法，通过三角化获取更多的共视点，通过PnP求出枢纽帧(4)和最后一帧(10)之间的每一帧(5—9)的位姿。
枢纽帧(4)与第一帧(0)之间的位姿和共视点求取按照同样的方法，先根据对极几何求出第0帧和第4帧的相对位姿，由于第4帧的位姿为( I , 0 ) (I,0)(I,0)，那么求出的相对位姿就是第0帧的位姿。然后通过三角化求出第0帧与第4帧之间的共视点，根据光流追踪的性质，这些点都会被第1—3帧所看到。然后通过PnP的方法求出第1帧的位姿，以此类推，通过三角化获取更多的共视点，通过PnP求出第2、3帧的位姿。
通过对滑窗进行上述操作，可求出滑窗中每一帧的位姿和所看到的3D共视点。
2) 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， 将feature的id以及有哪些帧可以观测到该feature以及在图像帧中的坐标。存储到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)的帧，通过对极约束求解本质矩阵分解出retative_pose。将该帧设置为枢纽帧。
/**
* 根据提供的特征点对计算旋转矩阵和平移向量
*/
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()求解essential矩阵，由于输入的匹配点已经去除了内参所以可以直接计算得到本质矩阵。由于本质矩阵只有五个自由度(t表示相对方向没有尺度)，理论只需要五对点就可以求解，但由于输入可能存在误匹配，这里使用了RANSEC，要求最少15个点对。最后通过recoverPose分解pose并判断内点个数是否大于12.
(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）理论
2) 流程
(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预积分的结果对齐之后，也就意味着本篇笔记开头的那张图中展示的视觉结构和IMU预积分结果匹配完成了。此时，我们需要获得世界坐标系中的位姿，也就是计算出PVQ，这样就完成了位姿的初始化估计，后边将用于进行单目紧耦合的VIO操作。
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]->repropagate()
使用尺度信息恢复视觉偏移及特征点深度；
使世界坐标系z轴对齐重力加速度方向~~，并将特征点转换到世界坐标系下~~
下一章 后端优化~~~~~~~~~
标签：tmp,int,Mono,frame,second,SFM,视觉,位姿,sfm From： https://blog.csdn.net/zoufeizhiba/article/details/144611381