这篇博客
这篇博客将介绍SVO论文中的Mapping部分,主要介绍深度滤波器在获取新的图像帧后,更新相应地图点深度的过程。(2020-6-18:修改“更新Seed对象”中的内容)
论文中的深度滤波
单目SLAM构建地图点是通过三角测量操作实现的。在SVO中,认为通过三角测量产生的地图点的深度值服从某种概率分布。在地图点刚构建时,这个概率分布的不确定性很大,即地图点深度值的不确定性较大(因为单目相机较难获得图中像素的真实深度)。这一点体现为下图中的浅绿色部分,即深度d在[dmin,dmax]中波动:
图中左边的方框是地图点的第一个观测帧(即产生该地图点的关键帧),右边是地图点的其他观测帧。将这两类帧分别记为 Ir 、Ik。
从图可以看出,降低地图点深度不确定性的方法是:针对一个由 Ir 产生的新地图点,在它的其他观测帧 Ik 上找到匹配的特征点(使用极线搜索的方法,极线为[dmin,dmax]在 Ik 上的投影线段)。以此构建 Ir 和 Ik 上的一对匹配特征,并使用这对匹配特征进行三角测量,获得该地图点新的深度测量值。将这新的测量值与地图点已有的深度信息融合,更新该深度值的分布情况,使[dmin,dmax]的范围变小。
当地图点的观测帧越多,它的深度值的波动范围就会越小。当其小到一定程度时,就可将该地图点放进地图中使用了。
上述就是SVO中深度滤波的大致概念,下面介绍它的代码流程。
深度滤波的代码流程
在SVO中,深度滤波被定义为一个单独的类对象。它将每个新构建的地图点设定为一个Seed对象(种子对象)。Seed对象包括了待优化的地图点、地图点深度的平均值和方差等信息。
深度滤波器通过观测帧来优化种子对象。当种子对象中深度值的方差足够小时,将该种子对应的地图点添加到地图中使用,否则继续进行优化。
在优化过程中,普通帧和关键帧对深度滤波的作用不同:
1、如果是普通帧,就用它来优化深度值未收敛的地图点;
2、如果是关键帧,还需要提取帧上新的特征点,并为这些特征点产生新的种子对象。
代码中使用 addFrame 和 addKeyFrame 两个函数处理不同的帧。在处理普通帧时会在addFrame函数中使用 updateSeeds 函数来更新种子对象:
更新Seed对象
//使用普通帧更新所有能够更新的Seed对象
//只更新一定数量的Seeds,不能在此消耗过多时间
void DepthFilter::updateSeeds(FramePtr frame)
{
size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size();
lock_t lock(seeds_mut_);
std::list<Seed>::iterator it=seeds_.begin();
const double focal_length = frame->cam_->errorMultiplier2();
//像素点的测量偏差
double px_noise = 1.0;
double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0;
while( it!=seeds_.end())
{
//判断种子更新是否需要终止
if(seeds_updating_halt_)
return;
//不去更新那些很老的Seed(产生这些Seed的关键帧ID比较老)
if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) {
it = seeds_.erase(it);
continue;
}
//检查待优化的地图点能否在当前帧上观测到
SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse();
const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
if(xyz_f.z() < 0.0) {
++it; // 投影点在相机成像面的后面
continue;
}
if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast<int>())) {
++it; // 投影点不再相机观测范围内
continue;
}
// we are using inverse depth coordinates使用逆深度进行参数化
float z_inv_min = it->mu + sqrt(it->sigma2);
float z_inv_max = max(it->mu - sqrt(it->sigma2), 0.00000001f);
double z;
if(!matcher_.findEpipolarMatchDirect(//因为是逆深度,所以用分式
*it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max, z))
//z是找到匹配后,三角化计算出来该地图点在参考帧中的深度
{
it->b++; // increase outlier probability when no match was found
++it;
++n_failed_matches;
continue;
}
// compute tau计算由一个像素测量偏差所带来的不确定度
double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle);
//计算测量值的标准差
double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
// update the estimate更新种子的概率分布。更新的是逆深度的概率分布
updateSeed(1./z, tau_inverse*tau_inverse, &*it);
++n_updates;
if(frame->isKeyframe())
{
//如果当前帧是关键帧,特征点检测时,不能在这个位置上(网格内)提取特征点
//因为这个位置已经找到了与地图点的匹配点,即已存在了一个特征
feature_detector_->setGridOccpuancy(matcher_.px_cur_);
}
//如果种子收敛了,就初始化一个候选地图点,并删除掉这个种子
if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh)
{
assert(it->ftr->point == NULL); // TODO this should not happen anymore
//注意是逆深度!!
Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));
//创建新地图点
Point* point = new Point(xyz_world, it->ftr);
//将参考帧中对应特征点指向这个新的地图点
it->ftr->point = point;
{
seed_converged_cb_(point, it->sigma2); // put in candidate list
}
it = seeds_.erase(it);
}
//种子的深度值的下限不确定,是个坏种子
else if(isnan(z_inv_min))
{
SVO_WARN_STREAM("z_min is NaN");
it = seeds_.erase(it);
}
else
++it;
}
}
在使用普通帧进行种子更新的过程中,有两个重要函数:1、updateSeed函数完成多个测量值的融合;2、computeTau函数计算观测帧中特征点的测量误差(代码中认为有一个像素坐标的测量误差)对地图点深度不确定性的影响。这两个函数是相关计算公式的实现,前者对应的公式和推导过程可以参考这篇博客;后者的公式推导可以参考《视觉SLAM十四讲》P326中的内容。(2020-6-18更新:)这里主要介绍updateSeed函数,因为computeTau和参考资料中的代码相似。
// 完成逆深度分布的融合。参数:新估计的逆深度 x 、x对应的方差 tau2 、x对应的 seed
void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
{
float norm_scale = sqrt(seed->sigma2 + tau2);
if(std::isnan(norm_scale))
return;
// 构建以norm_scale为方差的正态分布
boost::math::normal_distribution<float> nd(seed->mu, norm_scale);
// 1/(s^2) = 1/(sigma2)+ 1/(tau2)
float s2 = 1./(1./seed->sigma2 + 1./tau2);
// m = (s^2) * (mu/sigma2 + x/tau2)
float m = s2*(seed->mu/seed->sigma2 + x/tau2);
// C1 = a/(a+b) * N(x|mu,sigma2 + tau2) ,pdf()概率密度:正态分布nd在x处的概率密度
float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x);
// C2 = b/(a+b) * U(x)
float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range;
// C1、C2的归一化
float normalization_constant = C1 + C2;
C1 /= normalization_constant;
C2 /= normalization_constant;
// 计算 f、e 来进行a、b的更新
// f = C1*(a+1)/(a+b+1) + C2*(a)/(a+b+1)
float f = C1*(seed->a+1.)/(seed->a+seed->b+1.) + C2*seed->a/(seed->a+seed->b+1.);
float e = C1*(seed->a+1.)*(seed->a+2.)/((seed->a+seed->b+1.)*(seed->a+seed->b+2.))
+ C2*seed->a*(seed->a+1.0f)/((seed->a+seed->b+1.0f)*(seed->a+seed->b+2.0f));
// update parameters 参数更新
// mu = C1*m+C2*mu;
float mu_new = C1*m+C2*seed->mu;
seed->sigma2 = C1*(s2 + m*m) + C2*(seed->sigma2 + seed->mu*seed->mu) - mu_new*mu_new;
seed->mu = mu_new;
// a 和 b 的更新
seed->a = (e-f)/(f-e/f);
seed->b = seed->a*(1.0f-f)/f;
}
初始化Seed对象
在处理关键帧时,会在addKeyFrame函数中调用initializeSeed函数来初始化新的Seed对象:
//当有关键帧产生时,初始化新的Seed
void DepthFilter::initializeSeeds(FramePtr frame)
{
//从帧中提取特征
Features new_features;
feature_detector_->setExistingFeatures(frame->fts_);
feature_detector_->detect(frame.get(), frame->img_pyr_,
Config::triangMinCornerScore(), new_features);
//为每个新的特征初始化一个Seed对象
seeds_updating_halt_ = true;
lock_t lock(seeds_mut_); // by locking the updateSeeds function stops
++Seed::batch_counter;
//初始化新的Seed对象时,使用的初始深度是当前帧中观测到的地图点的平均深度
std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
});
if(options_.verbose)
//显示产生了多少个新的Seeds
SVO_INFO_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");
seeds_updating_halt_ = false;
}
结尾
感谢耐心的博友读到最后,希望这篇博客能帮助到各位。如果这篇博客的内容存在不足,希望大家指出,感谢大家的纠正。