6.2 代码解析
这部分代码在estimator::processImage()最后面。初始化部分的代码虽然生命周期比较短，但是，代码量巨大！主要分成2部分，第一部分是纯视觉SfM优化滑窗内的位姿，然后在融合IMU信息，按照理论部分优化各个状态量。
if (solver_flag == INITIAL)//进行初始化
{
if (frame_count == WINDOW_SIZE)
{
bool result = false;
if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
{
//确保有足够的frame参与初始化，有外参，且当前帧时间戳大于初始化时间戳+0.1秒
result = initialStructure();//执行视觉惯性联合初始化
initial_timestamp = header.stamp.toSec();//更新初始化时间戳
}
if(result)//初始化成功则进行一次非线性优化
{
solver_flag = NON_LINEAR;//进行非线性优化
solveOdometry();//执行非线性优化具体函数solveOdometry()
slideWindow();
f_manager.removeFailures();
ROS_INFO("Initialization finish!");
last_R = Rs[WINDOW_SIZE];//得到当前帧与第一帧的位姿
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
else
slideWindow();//不成功则进行滑窗操作
}
else
//TODO 再看看这个值是怎么变的
;//图像帧数量+1
}
6.3 initialStructure()
这是一个相当大的函数，而且多层套娃。而且原理上讲的初始化，包括纯视觉SfM，视觉和IMU的松耦合，都是在这个部分里面。
6.3.1 确保IMU有足够的excitation(可选)
这一部分的思想就是通过计算滑窗内所有帧的线加速度的标准差，判断IMU是否有充分运动激励，以判断是否进行初始化。
一上来就出现all_image_frame这个数据结构，见5.2-2，它包含了滑窗内所有帧的视觉和IMU信息，它是一个hash，以时间戳为索引。
(1)第一次循环，求出滑窗内的平均线加速度
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;//time for bk to bk+1
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);
(2)第二次循环，求出滑窗内的线加速度的标准差
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;
}
var = sqrt(var / ((int)all_image_frame.size() - 1));//计算加速度的标准差
//ROS_WARN("IMU variation %f!", var);
if(var < 0.25)
{
ROS_INFO("IMU excitation not enouth!");
//return false;
}
6.3.2 将f_manager中的所有feature在所有帧的归一化坐标保存到vector sfm_f中(辅助)
(1)一上来就定义了几个容器，分别是：
Quaterniond Q[frame_count + 1];
Vector3d T[frame_count + 1];
map<int, Vector3d> sfm_tracked_points;
vector<SFMFeature> sfm_f;
这块有2个需要注意的地方，
就是为什么容量是frame_count + 1？因为滑窗的容量是10，再加上当前最新帧，所以需要储存11帧的值！
然后出现了一个新的数据结构，我们看一下：
数据结构: vector<SFMFeature> sfm_f
它定义在initial/initial_sfm.h中，
struct SFMFeature
{
bool state;//状态（是否被三角化）
int id;
vector<pair<int,Vector2d>> observation;//所有观测到该特征点的 图像帧ID 和 特征点在这个图像帧的归一化坐标
double position[3];//在帧l下的空间坐标
double depth;//深度
};
可以发现，它存放着一个特征点的所有信息。容器定义完了，接下来就是往容器里放数据。
(2) 往容器里放数据
for (auto &it_per_id : f_manager.feature)//对于滑窗中出现的 所有特征点
{
int imu_j = it_per_id.start_frame - 1;//从start_frame开始帧编号
SFMFeature tmp_feature;
tmp_feature.state = false;//状态（是否被三角化）
tmp_feature.id = it_per_id.feature_id;//特征点id
for (auto &it_per_frame : it_per_id.feature_per_frame)//对于当前特征点 在每一帧的坐标
{
imu_j++;//帧编号+1
Vector3d pts_j = it_per_frame.point;//当前特征在编号为imu_j++的帧的归一化坐标
tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{
pts_j.x(), pts_j.y()}));//把当前特征在当前帧的坐标和当前帧的编号配上对
}//tmp_feature.observation里面存放着的一直是同一个特征，每一个pair是这个特征在 不同帧号 中的 归一化坐标
sfm_f.push_back(tmp_feature);//sfm_f里面存放着是不同特征
}
在这里，为什么要多此一举构造一个sfm_f而不是直接使用f_manager呢？
我的理解，是因为f_manager的信息量大于SfM所需的信息量(f_manager包含了大量的像素信息)，而且不同的数据结构是为了不同的业务服务的，所以在这里作者专门为SfM设计了一个全新的数据结构sfm_f，专业化服务。
6.3.3 在滑窗(0-9)中找到第一个满足要求的帧(第l帧)，它与最新一帧(frame_count=10)有足够的共视点和平行度，并求出这两帧之间的相对位置变化关系
(1)定义容器
Matrix3d relative_R;
Vector3d relative_T;
int l; //滑窗中满足与最新帧视差关系的那一帧的帧号
(2)两帧之间的视差判断,并得到两帧之间的相对位姿变化关系
if (!relativePose(relative_R, relative_T, l))
{
//这里的第L帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧l，会作为 参考帧 到下面的全局sfm使用，得到的Rt为当前帧到第l帧的坐标系变换Rt
ROS_INFO("Not enough features or parallax; Move device around");
return false;
}
这里，又出现了个新的函数relativePose()，这个函数也是6.3.3的主要功能，进去看一下：
首先，搞清楚这个函数是要干什么事情？
a.计算滑窗内的每一帧(0-9)与最新一帧(10)之间的视差，直到找出第一个满足要求的帧，作为我们的第l帧；
Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{
//output array R,t
// 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;
corres = f_manager.getCorresponding(i, WINDOW_SIZE);//寻找第i帧到窗口最后一帧(当前帧)的对应特征点归一化坐标
if (corres.size() > 20) //归一化坐标point(x,y,不需要z)
{
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));//改成3，4呢(对应像素坐标，1-3是归一化xyz坐标)
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
getCorresponding()的作用就是找到当前帧与最新一帧所有特征点在对应2帧下分别的归一化坐标，并配对，以供求出相对位姿时使用。
b.计算l帧与最新一帧的相对位姿关系
if(average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T))
{
//solveRelativeRT()通过基础矩阵计算当前帧与第l帧之间的R和T,并判断内点数目是否足够
l = i; //同时返回窗口最后一帧（当前帧）到第l帧（参考帧）的relative_R，relative_T
ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l);
return true;
//一旦这一帧与当前帧视差足够大了，那么就不再继续找下去了(再找只会和当前帧的视差越来越小）
}
这里最核心的公式就是m_estimator.solveRelativeRT()，这部分非常地关键。这里面代码很简单，就是把对应的点传进入，然后套cv的公式，但是求出来的R和T是谁转向谁的比较容易迷糊。
根据对以前学习内容和回忆和对后面公式的阅读，这个relative_R和relative_T是把最新一帧旋转到第l帧的旋转平移！
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; //因为这里的ll,rr是归一化坐标，所以得到的是本质矩阵
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
1674
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
