在estiamtor.cpp中
参考：VINS-Mono 代码解析二、初始化 第3部分_贵在坚持，不忘初心的博客-CSDN博客
VINS-mono代码阅读 -- 初始化_半亩园的博客-CSDN博客
//【4】进行初始化，一般初始化只进行一次；
if (solver_flag == INITIAL)
{
//frame_count是滑动窗口中图像帧的数量，一开始初始化为0，滑动窗口总帧数WINDOW_SIZE=10
if (frame_count == WINDOW_SIZE)
{
bool result = false;
//确保有足够的frame参与初始化，有外参，且当前帧时间戳大于初始化时间戳+0.1秒
if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
{
// 4.1 视觉惯性联合初始化-包括纯视觉SfM，视觉和IMU的松耦合
result = initialStructure();
// 4.2 更新初始化时间戳-
initial_timestamp = header.stamp.toSec();
}
if(result)//初始化成功则进行一次非线性优化
{
solver_flag = NON_LINEAR;// 先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿
solveOdometry(); // 执行非线性优化具体函数solveOdometry()
slideWindow(); // 滑动窗
// 4.5 剔除feature中估计失败的点（solve_flag == 2）0 haven't solve yet; 1 solve succ;
f_manager.removeFailures();
ROS_INFO("Initialization finish!");
// 4.6 初始化窗口中PVQ
last_R = Rs[WINDOW_SIZE];//得到当前帧与第一帧的位姿
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
else
slideWindow();// 初始化失败则直接滑动窗口
//因为只有在solveOdometry后才确定特征点的solve_flag(是否成功解算得到depth值)
}
else
frame_count++;// 图像帧数量+1，直到满足窗口数量
}为什么要初始化？
答：
- 视觉系统只能获得二维信息，损失了一维信息(深度), 所以需要动一下，也就是三角化才能重新获得损失的深度信息；
- 但是，这个三角化恢复的深度信息，是个“伪深度”，它的尺度是随机的，不是真实的，所以就需要IMU来标定这个尺度；
- 要想让IMU标定这个尺度，IMU也需要动一下（代码中求运动激励），得到PVQ的P；
- 另外，IMU存在bias，视觉获得的旋转矩阵不存在bias，所以可以用视觉来标定IMU的旋转bias；
- 需要获得世界坐标系这个先验信息，通过初始化能借助g来确定；
本文主要介绍initialStructure部分！！！
1. 初始化进行的条件
（1）solver_flag == INITIAL
（2）frame_count == WINDOW_SIZE
（3）外参初始化成功，并且距离上一次初始化的时间大于0.1
2. initialStructure 初始化部分
2.1 第一部分：通过计算标准差来判断imu的激励是否足够
思路：遍历除了第一帧图像以外的所有帧，由各帧预积分得到的delta_v / dt表示加速度，求（k-1）帧图像之间加速度的标准差，来判断imu是否充分运动。
{
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;
//计算平均加速度
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 + (x2 - x)^2 + ... + (xn - x)^2]/n
* 标准差： s = sqrt(s^2)
*/
var = sqrt(var / ((int)all_image_frame.size() - 1));
//通过加速度标准差判断IMU是否由充分运动，标准差必须大于等于0.25
}
2.2 把特征点和共视帧信息提取出来
// ********* global sfm ********
Quaterniond Q[frame_count + 1];
Vector3d T[frame_count + 1];
map<int, Vector3d> sfm_tracked_points;//用于存储sfm重建出来的特征点坐标，所有观测到该特征点的图像的id和图像坐标
vector<SFMFeature> sfm_f;
遍历每一个f_manager中的特征点
for (auto &it_per_id : f_manager.feature)// list<FeaturePerId> feature;
{
}
sfm_f是一个容器（向量（Vector）是一个封装了动态大小数组的顺序容器），里面存储的类型是SFMFeature，这是一个结构体：
循环内部：
（1）把当前循环到的f_manager.feature（用it_per_id表示）的特征id赋值给新建的结构体tmp_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; //就是特征点的id
(2)遍历每一个能观察到该特征的图像帧
把该特征在该图像帧中的坐标赋值给pts_j；
然后把图像帧的序号，坐标打包赋值给tmp_feature的observation
tmp_feature里面记录了该特征在不同图像帧内的state,id,observation
-
把当前特征在当前帧的坐标和当前帧的编号配上对
-
tmp_feature.observation里面存放着的一直是同一个特征，每一个pair是这个特征在 不同帧号 中的 归一化坐标
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;//后面要++，前面为啥要-1呢？ 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()}));
}
(3) 把tmp_feature加到sfm_f中。
sfm_f.push_back(tmp_feature);sfm_f里面存放着是不同特征
这里的关系能画一个类似特征管理器中的包括图就好啦
2.3 找到满足要求的第L帧，并求和最后一帧的RT
在滑窗中找到第一个满足要求的帧(第L帧)，它与最新一帧有足够的共视点和平行度，由此求出这两帧之间的相对位姿
计算滑窗内的每一帧(0-9)与最新一帧(10)之间的视差，直到找出第一个满足要求的帧，作为我们的第L帧
Matrix3d relative_R;
Vector3d relative_T;
int l;
//通过求解本质矩阵来求解位姿
if (!relativePose(relative_R, relative_T, l)) //计算的结果给relative_R, relative_T
{
ROS_INFO("Not enough features or parallax; Move device around");
return false;
}
relativePose的程序：
这里的第L帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧，作为参考帧到下面的全局sfm使用，得到的 R t 为当前帧到第L帧的坐标变换 R t
该函数判断滑动窗口中第一帧到最后一帧，对应特征点的平均视差大于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);
if (corres.size() > 20)
{
}
}
return false;
}
（1）getCorresponding()
参考：VINS-Mono代码阅读笔记：getCorresponding()_一学子的博客-CSDN博客
作用：找出最新的两帧之间共视特征点的对应points有哪些（归一化坐标）
得到frame_count_l与frame_count_r两帧之间的对应特征点
传入的参数：frame_count_l 为frame_count-1，倒数第二帧
frame_count_r为frame_count，最新帧
vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)
{
//存储3d点对的容器列表
vector<pair<Vector3d, Vector3d>> corres;
for (auto &it : feature)
{
//判断每个特征点的对应的frame是否在有效范围内
if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)
{
Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();
//计算传入的frame_count_l和frame_count_r距离最开始一帧的距离，也就是最新两帧的index
int idx_l = frame_count_l - it.start_frame;
int idx_r = frame_count_r - it.start_frame;
a = it.feature_per_frame[idx_l].point;
b = it.feature_per_frame[idx_r].point;
//corres中存放的就是每一个特征it在两个帧中对应的point点对
corres.push_back(make_pair(a, b));
}
}
return corres;
}
遍历滑动窗口内整个图像帧，
找到第i帧到最后一帧图像共同特征点的坐标对，存放到corres中。
（2）当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));
//norm是斜边的距离
double parallax = (pts_0 - pts_1).norm();
sum_parallax = sum_parallax + parallax;
}
//平均视差
average_parallax = 1.0 * sum_parallax / int(corres.size());
//判断是否满足初始化条件：视差> 30， 内点数>12 solveRelativeRT 返回true
//solveRelativePoseRtT() 通过基础矩阵计算当前帧与第l帧的 R 和 T ，并判断内点数是否足够
//同时返回窗口最后一帧（当前帧）到第l帧（参考帧）的relative_R 和relativeT
if(average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T))
{
l = i;
ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l);
return true;
}c++ 里面的map容器的迭代器里面 有个first 和 second
例如
map<string, int> m;
m["one"] = 1;map<string, int>::iterator p = m.begin();
p->first; // 这个是 string 值是 "one"
p->second; //这个是 int 值是 1
原文链接：https://blog.csdn.net/myachilies/article/details/8872111
这里的ceres是由一个vector3d对组成的。
这里也就是说，对于第i帧和最后一帧间，如果由超过20个共视特征点，则求这些特征点间的距离（也就是视差），然后求平均视差。如果平均视差*460 >30 (为什么要*460？) 且solveRelativeRT返回true，输出相关信息，到此结束；如果所有的帧都不能满足要求，就返回false。
（3）计算solveRelativeRT
这个函数里主要就是调用了openCV库的函数findFundamentalMat来计算fundamental matrix，然后再用recoverPose根据fundamental matrix恢复相机的位姿。这里会得到一个当前帧到参考帧的旋转和平移。
bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
if (corres.size() >= 15)
{
//相当于把corres中的共视特征点坐标（x,y）拿了出来，放到了ll和rr中
//ll 应该是第L帧，rr是最后一帧
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, //计算基础矩阵的方法，这里使用的是8点法
* double ransacReprojThreshold = 3, //点到对极线的最大距离，超过这个值的点将被舍弃
* double confidence = 0.99, //矩阵正确的可信度
* OutputArray mask = noArray(), //输出在计算过程中没有被舍弃的点
*)
*/
cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
//相机矩阵设置为单位矩阵
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() //在 findFundamentalMatrix() 中被丢弃的点
*)
*/
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循环就是把rot各元素赋值给R矩阵
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;
if(inlier_cnt > 12) //内点数大于12？
return true;
else
return false;
}
return false;
}
输入就是共视特征点对，R，T的引用还是指针？
cv：：point2f 参考：2维（x,y轴）关于cv::Point2d cv::Point2f cv:Point2i cv::Point2l cv::Point3d cv::Point3f cv::Point3i的见解_枪枪枪的博客-CSDN博客_point2f
根据共视特征点坐标，求本质矩阵。再利用recover_pose函数从本质矩阵中回复旋转和平移。好像不能直接恢复成R和t？
这里有个问题，看recoverPose要使用的是essential matrix，而这里传入的确实fundamental matrix，是因为这里用的是归一化的坐标，所以相机的内参矩阵用的是单位阵，在这样的情况下，essential matrix 是等于 fundamental matrix的。
recoverPose参考：OpenCV中cv::recoverPose()函数详细介绍和用法，以及求解出的R,t的坐标相对关系_一点儿也不萌的萌萌的博客-CSDN博客_opencv recoverpose
这里求得的是第i帧到最后一帧的RT，然后转置？？？
2.4 GlobalSFM sfm
参考：VINS-Mono 代码详细解读6——视觉SFM详解 initialStructure（） - 知乎
//construct()：对窗口中每个图像帧求解sfm问题，
//得到所有图像帧相对于参考帧l的姿态四元数Q、平移向量T和特征点坐标sfm_tracked_points （核心）
GlobalSFM sfm;
if(!sfm.construct(frame_count + 1, Q, T, l,
relative_R, relative_T,
sfm_f, sfm_tracked_points))
{
//这说明了在初始化后期的第一次slidingwindow() marg掉的是old帧。
ROS_DEBUG("global SFM failed!");
marginalization_flag = MARGIN_OLD;
return false;
}
/**
* @brief 纯视觉sfm，求解窗口中的所有图像帧相对于第l帧的位姿和三角化的特征点坐标
* @param[in] frame_num 窗口总帧数（frame_count + 1）
* @param[out] q 窗口内图像帧的旋转四元数q（相对于第l帧）Q[frame_count + 1]
* @param[out] T 窗口内图像帧的平移向量T（相对于第l帧）
* @param[in] l 第l帧
* @param[in] relative_R 当前帧到第l帧的旋转矩阵
* @param[in] relative_T 当前帧到第l帧的平移向量
* @param[in] sfm_f 所有特征点
* @param[out] sfm_tracked_points 所有在sfm中三角化的特征点ID和坐标
* @return bool true:sfm求解成功
*/
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)这里为什么是当前帧到第L帧的RT？
（1）以第L帧为参考帧，建立当前帧到最新一帧的旋转和平移
feature_num = sfm_f.size();
// intial two view
//第l帧为参考帧，建立第L帧的RT
q[l].w() = 1;
q[l].x() = 0;
q[l].y() = 0;
q[l].z() = 0;
T[l].setZero();
//往q中存入最新帧的旋转
//relative_R和relative_T为当前帧到第L帧的旋转和平移
q[frame_num - 1] = q[l] * Quaterniond(relative_R);
T[frame_num - 1] = relative_T;
（2）将第L帧和最新帧相对于第L帧的RT取转置，存于pose中。
//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];
//R T取转置
c_Quat[l] = q[l].inverse();
c_Rotation[l] = c_Quat[l].toRotationMatrix();
c_Translation[l] = -1 * (c_Rotation[l] * T[l]);
//将第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_Quat存放q[l]的逆
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 中
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];
（3）PNP+三角测量
求从第L帧到最新帧之间的所有帧到第L帧之间的RT
for (int i = l; i < frame_num - 1 ; i++)
{
if (i > l)
{
Matrix3d R_initial = c_Rotation[i - 1];
Vector3d P_initial = c_Translation[i - 1];
// 求解PnP得到RT
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];
}
// l,l+1,l+2.....framenum-2和framenum-1 三角化恢复路标3D坐标
triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
}a. solveFrameByPnP（）函数得到第i帧的RT
/* @brief PNP方法得到第l帧到第i帧的位姿
* @param[in] i 第i帧
* @param[update] R_initial、P_initial、sfm_f
*/
bool GlobalSFM::solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i,
vector<SFMFeature> &sfm_f)
{
vector<cv::Point2f> pts_2_vector;
vector<cv::Point3f> pts_3_vector;
// 1 要把待求帧i上所有特征点的归一化坐标和3D坐标(l系上)都找出来
for (int j = 0; j < feature_num; j++)//feature_num = sfm_f.size()
{
if (sfm_f[j].state != true)// 检查状态，该点是否被三角化
continue;
Vector2d point2d;
for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)
{
if (sfm_f[j].observation[k].first == i)// 判断图像帧索引是否相同
{
// 通过观测得到2D像素点
Vector2d img_pts = sfm_f[j].observation[k].second;
cv::Point2f pts_2(img_pts(0), img_pts(1));
pts_2_vector.push_back(pts_2);
// 通过position得到路标3D坐标
cv::Point3f pts_3(sfm_f[j].position[0], sfm_f[j].position[1], sfm_f[j].position[2]);
pts_3_vector.push_back(pts_3);
break;
}
}
}
if (int(pts_2_vector.size()) < 15)// 判断像素点数量是否足够
{
printf("unstable features tracking, please slowly move you device!\n");
if (int(pts_2_vector.size()) < 10)
return false;
}
// 2 根据匹配的特征点和3D坐标 用PnP求RT
cv::Mat r, rvec, t, D, tmp_r;
cv::eigen2cv(R_initial, tmp_r);
cv::Rodrigues(tmp_r, rvec);
cv::eigen2cv(P_initial, t);
cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
bool pnp_succ;
pnp_succ = cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1);
if(!pnp_succ)
{
return false;
}
cv::Rodrigues(rvec, r);
//cout << "r " << endl << r << endl;
MatrixXd R_pnp;
cv::cv2eigen(r, R_pnp);
MatrixXd T_pnp;
cv::cv2eigen(t, T_pnp);
R_initial = R_pnp;
P_initial = T_pnp;
return true;
}b. 求得两帧间的RT后，再用三角测量求其他特征点的三维坐标
/* @brief 三角化frame0和frame1间所有对应点
* @param[in] frame,Pose 帧索引和位姿数据
* @param[out] sfm_f的state和position 3D坐标
*/
//
void GlobalSFM::triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0,
int frame1, Eigen::Matrix<double, 3, 4> &Pose1,
vector<SFMFeature> &sfm_f)
{
assert(frame0 != frame1);
for (int j = 0; j < feature_num; j++)// 遍历所有特征点
{
if (sfm_f[j].state == true) // 没有三角化的话才开始三角化
continue;
//
bool has_0 = false, has_1 = false;
Vector2d point0; Vector2d point1;
for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)
{ // 找到对应的帧索引，取出2D像素坐标
if (sfm_f[j].observation[k].first == frame0)
{
point0 = sfm_f[j].observation[k].second;
has_0 = true;
}
if (sfm_f[j].observation[k].first == frame1)
{
point1 = sfm_f[j].observation[k].second;
has_1 = true;
}
}
// 如果2D像素坐标都匹配上了，三角化
if (has_0 && has_1)
{
Vector3d point_3d;
triangulatePoint(Pose0, Pose1, 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 : " << frame1 << " 3d point : " << j << " " << point_3d.transpose() << endl;
}
}
}
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
