这篇博客

这篇博客将介绍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)//显示产生了多少个新的SeedsSVO_INFO_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");seeds_updating_halt_ = false;
}

结尾

感谢耐心的博友读到最后，希望这篇博客能帮助到各位。如果这篇博客的内容存在不足，希望大家指出，感谢大家的纠正。