临时记录:
考虑的问题是:滑动窗口没有满时,我们把第一帧作为参考帧,滑动窗口不断向前走,新的帧加入和旧的帧剔除,
这时候的参考帧是谁?
答:在滑窗内寻找与当前帧的匹配特征点数较多的关键帧作为参考帧, 并通过求基础矩阵
cv::findFundamentalMat 计算出当前帧到参考帧的 T;
一、原因
为了保证系统的实时性和准确性,需要对当前帧之前某一部分帧进行优化,而不是全部历史帧,优化帧的个数便是滑动窗口的大小。
不难理解,为了维持窗口大小,要去除旧的帧添加新的帧,即边缘化 Marginalization。到底是删去最旧的帧(MARGIN_OLD)还是
删去刚刚进来窗口倒数第二帧(MARGIN_SECOND_NEW),就需要对 当前帧与之前帧 进行视差比较,如果是当前帧变化很小,就会删
去倒数第二帧,如果变化很大,就删去最旧的帧 --- 理解一下这个意思!
二、关键帧选取规则
1、当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧(因为视差可以根据平移和旋转共同得到,而纯旋转则导致不能三角化成功,所以这一步需要IMU预积分进行补偿)
2、当前帧跟踪到的特征点数量小于阈值视为关键帧;
三、视差检查程序位置
自己想一下,应该在哪个位置?当然是来一张图片检查一次了,逻辑很清晰;程序在 processImage 内,也就是当 estimator 得到一帧图
片的信息后,就会立马进行处理,首先第一步就是添加图片内的特征点以及检查视差, 程序如下:
/------------------------------------------------------------程序--------------------------------------------------------------------/
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double header)
{
ROS_DEBUG("new image coming ------------------------------------------");
ROS_DEBUG("Adding feature points %lu", image.size());
//视差检查,选者要边缘化掉的帧
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
{
marginalization_flag = MARGIN_OLD;
}
else
{
marginalization_flag = MARGIN_SECOND_NEW;
}
检查视差代码: f_manager.addFeatureCheckParallax(frame_count, image, td)
想一下,这个过程是滑动差窗口进来图像,检测特征点的过程,这个新进来的图像对应的特征点 也会被其他以前的图像观测到,
程序中,所谓的其他以前的图像, 指的就是 至少倒数第三帧或以前 和 倒数第二帧以后,新进来的图像是倒数第一帧,但是我们
还没有检测这个帧是不是关键帧,要不要剔除,这个程序就是干这个事情!判断观测到该特征点的frame中倒数第二帧和倒数第
三帧的共视关系 实际是求取该特征点在两帧的归一化平面上的坐标点的距离ans
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
ROS_DEBUG("input feature: %d", (int)image.size());
ROS_DEBUG("num of feature: %d", getFeatureCount());
//所有特征点视差总和
double parallax_sum = 0;
// 满足某些条件的特征点个数
int parallax_num = 0;
//被跟踪点的个数
last_track_num = 0;
for (auto &id_pts : image)
{
//特征点管理器,存储特征点格式:首先按照特征点ID,一个一个存储,每个ID会包含其在不同帧上的位置
FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
int feature_id = id_pts.first;
// find_if 函数,找到一个interator使第三个仿函数参数为真
auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
{
return it.feature_id == feature_id;
} );
if (it == feature.end())
{
//如果没有找到此ID,就在管理器中增加此特征点
feature.push_back(FeaturePerId(feature_id, frame_count));
feature.back().feature_per_frame.push_back(f_per_fra);
}
else if (it->feature_id == feature_id)
{
//如果找到了相同ID特征点,就在其FeaturePerFrame内增加此特征点在此帧的位置以及其他信息,然后增加last_track_num,说明此帧有多少个相同特征点被跟踪到
it->feature_per_frame.push_back(f_per_fra);
last_track_num++;
}
}
if (frame_count < 2 || last_track_num < 20)
return true;
for (auto &it_per_id : feature)
{
//计算能被当前帧和其前两帧共同看到的特征点视差
if (it_per_id.start_frame <= frame_count - 2 &&
it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
{
parallax_sum += compensatedParallax2(it_per_id, frame_count);
parallax_num++;
}
}
if (parallax_num == 0)
{
return true;
}
else
{
ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
return parallax_sum / parallax_num >= MIN_PARALLAX;
}
}
子程序:每个特征点视差计算 compensatedParallax2(it_per_id, frame_count)
double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
{
//check the second last frame is keyframe or not
//parallax betwwen seconde last frame and third last frame
const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];
const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame];
double ans = 0;
Vector3d p_j = frame_j.point;// 3D路标点(倒数第二帧j)
double u_j = p_j(0);
double v_j = p_j(1);
Vector3d p_i = frame_i.point;// 3D路标点(倒数第三帧i)
Vector3d p_i_comp;
//int r_i = frame_count - 2;
//int r_j = frame_count - 1;
//p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;
p_i_comp = p_i;
double dep_i = p_i(2);
double u_i = p_i(0) / dep_i;
double v_i = p_i(1) / dep_i;
double du = u_i - u_j, dv = v_i - v_j;
//没有惯导补偿,这一部分重复了
double dep_i_comp = p_i_comp(2);
double u_i_comp = p_i_comp(0) / dep_i_comp;
double v_i_comp = p_i_comp(1) / dep_i_comp;
double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;
ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));
return ans;
}
/------------------------------------------------------------程序--------------------------------------------------------------------/
四、理解
前面的程序已经很清楚了,不过个人感觉最繁琐的地方不是程序算法有多难理解,而是变量,需要清楚特征点的存储关系,
这里我再次上一波关系图,如下所示:
传到 processImage 的 image 表示 当前帧跟踪到上一帧中的特征点集合, 也就是当前帧观测到的所有的路标点(不包括在当前帧新提取的点)
,如表 2 中 track_cnt≥2 的 feature, image 类型为:
FeatureManager 中几个变量:
对于 compensatedParallax2(it_per_id, frame_count) 这个程序,明显是为了求 两帧中对应特征点的距离;
但是作者把 惯导补偿 对应的程序删除了,导致后面有一段程序是重复的!也没有很理解,或者我们就应该有质疑的精神,
他们是很厉害,可是也不是完美,这个地方就是不足之处!
再来说一下如果加上惯导补偿,示意图如下:
注意:
Ps 、Vs 、Rs 代表的意义可能很多人还没有清楚,这个强调多少次都不为过!这个值是有IMU机械编排出来的,
代表的是 b(k) 坐标系相对与世界坐标系的位置、速度、姿态,而这个k是对应的相机帧,因为相机和IMU是固连的呀!
而相机和IMU通过Rb_c进行转换的,也就是我们自动外参标定的部分! 仔细理解上面这个图的转换关系!