SVO2系列之深度滤波DepthFilter

1. 前言

在svo2.0即rpg_svo_pro_open中，深度滤波模块有一些变化，提供了不只是George Vogiatzis作者提出的Gaussian+ Uniform组合模型，还实现Gaussian模型来供选择更新深度滤波状态，另外代码函数名称等也有变化，本篇着重详细解析svo2.0中的深度滤波算法思路和推导。下面分几个部分来讲解：

DepthFilter滤波器对象创建
关键帧与普通帧处理差异
svo2.0中的深度滤波核心算法流程、推导及解析（着重讲解）
更新地图点

2. DepthFilter对象创建

在frame_handler_base.cpp的构造函数中创建depth_filter_深度滤波器

FrameHandlerBase::FrameHandlerBase(const BaseOptions &base_options,const ReprojectorOptions &reprojector_options,const DepthFilterOptions &depthfilter_options,const DetectorOptions &detector_options,const InitializationOptions &init_options,const FeatureTrackerOptions &tracker_options,const CameraBundle::Ptr &cameras): options_(base_options), cams_(cameras), stage_(Stage::kPaused),set_reset_(false), set_start_(false), map_(new Map),acc_frame_timings_(10), acc_num_obs_(10), num_obs_last_(0),tracking_quality_(TrackingQuality::kInsufficient),relocalization_n_trials_(0) {..........depth_filter_.reset(new DepthFilter(depthfilter_options, detector_options2, cams_));initializer_ = initialization_utils::makeInitializer(init_options, tracker_options, detector_options, cams_);overlap_kfs_.resize(cams_->getNumCameras());VLOG(1) << "SVO initialized";
}

在DepthFilter内部构造函数中，如果配置文件中设置多线程深度滤波则会开启一个线程来计算updateSeedsLoop()，以下的内容都以多线程的设置模式为例进行讲解。

void DepthFilter::startThread()
{if(thread_){SVO_ERROR_STREAM("DepthFilter: Thread already started!");return;}SVO_INFO_STREAM("DepthFilter: Start thread.");thread_.reset(new std::thread(&DepthFilter::updateSeedsLoop, this));
}

3. 关键帧与普通帧处理差异

在updateSeedsLoop()中是深度滤波计算两种过程（SEED_INIT和UPDATE），其不断从DepthFilter::JobQueue队列中取当前图像帧数据来完成计算。不管是关键帧还是普通帧，都会使用当前帧深度更新与其有共视关系的历史关键帧overlap_kfs上种子点seeds的概率，只不过关键帧进入队列时会清空队列内所有数据来保证该关键帧处理。队列中关键帧和普通帧这两种图像帧数据，处理差异具体如下：

void DepthFilter::updateSeedsLoop()
{while(true){// wait for new jobsJob job;{ulock_t lock(jobs_mut_);while(jobs_.empty() && !quit_thread_)jobs_condvar_.wait(lock);if(quit_thread_)return;job = jobs_.front();jobs_.pop();} // release lock// process jobsif(job.type == Job::SEED_INIT){ulock_t lock(feature_detector_mut_);depth_filter_utils::initializeSeeds(job.cur_frame, feature_detector_,options_.max_n_seeds_per_frame,job.min_depth, job.max_depth, job.mean_depth);if (options_.extra_map_points){for (size_t idx = 0; idx < job.cur_frame->numFeatures(); idx++){const FeatureType& type = job.cur_frame->type_vec_[idx];if (!isMapPoint(type) && type != FeatureType::kOutlier){sec_feature_detector_->closeness_check_grid_.fillWithKeypoints(job.cur_frame->px_vec_.col(static_cast<int>(idx)));}}depth_filter_utils::initializeSeeds(job.cur_frame, sec_feature_detector_,options_.max_n_seeds_per_frame + options_.max_map_seeds_per_frame,job.min_depth, job.max_depth, job.mean_depth);}}else if(job.type == Job::UPDATE){// We get higher precision (10x in the synthetic blender dataset)// when we keep updating seeds even though they are converged until// the frame handler selects a new keyframe.depth_filter_utils::updateSeed(*job.cur_frame, *job.ref_frame, job.ref_frame_seed_index, *matcher_,options_.seed_convergence_sigma2_thresh, true, false);}}
}

3.1 关键帧：

会通过addKeyframe()先清空JobQueue队列，进而往JobQueue队列中塞入当前帧进行后续深度滤波计算(保证实时性)
塞入Job属性为Job::SEED_INIT，初始化其所有特征为Seed类别属性如：kCorner -> kCornerSeed ，kEdgelet -> kEdgeletSeed，kMapPoint -> kMapPointSeed;并且初始化其种子点seeds的深度值seed_mu_range_、地图点逆深度平均值及方差信息(invmu_sigma2_a_b_vec_中)等均在initializeSeeds()函数中实现

3.2 普通帧：

会仅调用updateSeeds()来塞入JobQueue队列进行后续深度滤波计算，塞入的Job属性为Job::UPDATE，这里不初始化新的seed种子点个人认为主要是起到加速作用
processFrame接口代码：

UpdateResult FrameHandlerStereo::processFrame() {// ---------------------------------------------------------------------------// trackingstd::cout << "STEP 1: Sparse Image Align !" << std::endl;// STEP 1: Sparse Image Alignsize_t n_tracked_features = 0;sparseImageAlignment();// STEP 2: Map Reprojection & Feature Alignn_tracked_features = projectMapInFrame();if (n_tracked_features < options_.quality_min_fts) {return makeKeyframe(); // force stereo triangulation to recover}// STEP 3: Pose & Structure Optimizationif (bundle_adjustment_type_ != BundleAdjustmentType::kCeres) {n_tracked_features = optimizePose();if (n_tracked_features < options_.quality_min_fts) {return makeKeyframe(); // force stereo triangulation to recover}optimizeStructure(new_frames_, options_.structure_optimization_max_pts, 5);}// return if tracking badsetTrackingQuality(n_tracked_features);if (tracking_quality_ == TrackingQuality::kInsufficient) {return makeKeyframe(); // force stereo triangulation to recover}// ---------------------------------------------------------------------------// select keyframeframe_utils::getSceneDepth(new_frames_->at(0), depth_median_, depth_min_,depth_max_);if (!need_new_kf_(new_frames_->at(0)->T_f_w_)) {for (size_t i = 0; i < new_frames_->size(); ++i)**depth_filter_->updateSeeds(overlap_kfs_.at(i), new_frames_->at(i));**return UpdateResult::kDefault;}SVO_DEBUG_STREAM("New keyframe selected.");return makeKeyframe();
}

其中，关键帧插入JobQueue队列，相关代码如下：

//Init新特征点，塞入JobQueue队列且属性为Job::SEED_INIT
depth_filter_->addKeyframe(new_frames_->at(0), depth_median_, 0.5*depth_min_, depth_median_*1.5);//使用当前关键帧深度滤波更新共视关系的历史关键帧上seeds概率，塞入JobQueue队列且属性为Job::UPDATE
depth_filter_->updateSeeds(overlap_kfs_.at(0), new_frames_->at(0));
//使用new_frames_->at(0)来更新overlap_kfs_.at(0)种子点seed(种子点只在关键帧时才初始提取出的，
//则这里只更新了关键帧上的种子点概率)
depth_filter_->updateSeeds(overlap_kfs_.at(1), new_frames_->at(1));
//使用new_frames_->at(1)来更新

非关键帧(普通帧)插入JobQueue队列，相关代码如下：这个操作在processFrame接口中如上，因此所有的帧，都要经过这个步骤的处理

//使用当前普通帧DepthFilter更新共视关系的历史关键帧seeds概率，塞入JobQueue队列且属性为Job::UPDATE
depth_filter_->updateSeeds(overlap_kfs_.at(i), new_frames_->at(i));

对于关键帧和普通帧的处理流程相同之处就是在Job::UPDATE模式下进行种子点seeds更新，具体在updateSeed()中实现。

4. 深度滤波核心算法流程、推导及解析

4.1 算法流程

svo论文中的关于深度滤波的一个示意图，如图所示

深度滤波处理前已知：

ref_frames以及cur_frame在world下位姿T
其特征点对应的world系下3d点(深度信息depth)

在svo计算过程中有维护一个子地图Map（所有关键帧上特征点对应的世界系下3D点集）。从上面几个模块分析可得种子点都是从关键帧init出来的，如kCornerSeed，普通帧只用其特征点（如kCorner）来更新具有共视关系的关键帧上的种子点（seeds）的深度概率分布，每个种子点都各有一个深度概率分布去维护和计算，核心的深度滤波算法流程主要是以下几个过程及其对应的核心接口实现：

加载关键帧信息到Job队列，并提取新的seeds。初始化所有种子点seed初始深度信息depth、深度分布平均值mu、深度值分布方差sigma2、Beta分布计算中的a和b值，这里注意只针对关键帧。（在initializeSeeds()函数中实现)
循环遍历所有与cur_frame有共视关系的各个overlap_kfs_的seeds，进行深度滤波算法计算，这里注意已经收敛的Converaged点不进入概率更新计算。进入 depth_filter_utils::updateSeed接口中
每个ref_frame上的seed会创建FeatureWrapper，与cur_frame进行极线匹配搜索算法计算findEpipolarMatchDirect()，得到px_cur_、f_cur_和ref_frame上该种子点depth
计算不确定度 pi ，**代码实现是在computeTau()**中，svo2.0中不确定度计算基本和高博<<视觉十四讲第2版>>中p312页完全一致，这里不再详细解释
使用vogiatzis方法时，更新深度概率分布的平均值 mu和方差sigma^2，以及更新Beta 分布中的 a和b（在updateFilterVogiatzis()函数中实现）
如果不使用vogiatzis方法时，仅更新深度概率分布的平均值 mu 和方差 sigma2 (在updateFilterGaussian()函数中实现)
最后用最新计算的 depth 更新地图点 (在upgradeSeedsToFeatures()函数中实现)

注意：上面步骤2中会判断是否为种子点，只有是种子点才进行深度滤波概率更新，深度滤波本质上是一个匹配的过程，类似与基于特征点法的描述子的功能，目的是实现不同图像帧率上的特征关联。可以简单的理解为一种特征关联的方法。
在深度滤波过程中还要注意作者用如下操作完成引用，并通过后面深度滤波算法更新state从而更新invmu_sigma2_a_b_vec_信息，具体实现如下，基于eigen的左值引用实现

Eigen::Ref<SeedState> state = ref_frame.invmu_sigma2_a_b_vec_.col(seed_index);

另外注意上面步骤5和步骤6，区别于svo1.0，作者使用两种模型来供选择：

一种使用vogiatzis提出的Gaussian+ Uniform混合模型
第二种可以选择使用单独Gaussian模型

接口集体实现在 depth_filter_utils::updateSeed 接口中，模型选择对应的代码如下：

 // update the estimateif(use_vogiatzis_update){if(!updateFilterVogiatzis(seed::getMeanFromDepth(depth),seed::getSigma2FromDepthSigma(depth, depth_sigma),ref_frame.seed_mu_range_,state)){ref_ftr.type = FeatureType::kOutlier;return false;}}else{if(!updateFilterGaussian(seed::getMeanFromDepth(depth),seed::getSigma2FromDepthSigma(depth, depth_sigma),state)){ref_ftr.type = FeatureType::kOutlier;return false;}}

4.2 算法流程

关于推导，结合svo论文、George Vogiatzis的《Video-based, Real-Time Multi View Stereo》论文、以及贺博的博客SVO原理解析，这里据说还有一个大佬在stackoverflows 上面的精彩解答，不过这篇作者整理出来的已经够用。白话解析推导：

bool updateFilterVogiatzis(const FloatType z, // Measurementconst FloatType tau2,const FloatType mu_range,Eigen::Ref<SeedState>& mu_sigma2_a_b)//svo1.0 的DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
{FloatType& mu = mu_sigma2_a_b(0);FloatType& sigma2 = mu_sigma2_a_b(1);FloatType& a = mu_sigma2_a_b(2);FloatType& b = mu_sigma2_a_b(3);const FloatType norm_scale = std::sqrt(sigma2 + tau2);if(std::isnan(norm_scale)){LOG(WARNING) << "Update Seed: Sigma2+Tau2 is NaN";return false;}const FloatType oldsigma2 = sigma2;const FloatType s2 = 1.0/(1.0/sigma2 + 1.0/tau2);//旧的s    ( | , 2) 旧的的方差s2  对应公式14中的s2 tau2为不确定度  (不确定性 )const FloatType m = s2*(mu/sigma2 + z/tau2);//旧的m   ( | , 2)  对应公式14中的mconst FloatType uniform_x = 1.0/mu_range;//U(x| z_min,z_max)FloatType C1 = a/(a+b) * vk::normPdf<FloatType>(z, mu, norm_scale);//  c1 = ( / + ) ( | , 2+ 2)FloatType C2 = b/(a+b) * uniform_x;const FloatType normalization_constant = C1 + C2;C1 /= normalization_constant;C2 /= normalization_constant;const FloatType f = C1*(a+1.0)/(a+b+1.0) + C2*a/(a+b+1.0);const FloatType e = C1*(a+1.0)*(a+2.0)/((a+b+1.0)*(a+b+2.0))+ C2*a*(a+1.0)/((a+b+1.0)*(a+b+2.0));// update parametersconst FloatType mu_new = C1*m+C2*mu;//新的平均值 对应公式35sigma2 = C1*(s2 + m*m) + C2*(sigma2 + mu*mu) - mu_new*mu_new;//对应公式36mu = mu_new;//a = (e - f) / (f - e / f);//对应公式33b = a * (1.0 - f) / f;//对应公式34// TODO: This happens sometimes.if(sigma2 < 0.0) {LOG(WARNING) << "Seed sigma2 is negative!";sigma2 = oldsigma2;}if(mu < 0.0) {LOG(WARNING) << "Seed diverged! mu is negative!!";mu = 1.0;return false;}return true;
}

另外，如果use_vogiatzis_update = false时，通过代码中的实现，推测出svo2.0作者这里只使用高斯模型分布更新来计算，我们重写式(10)，去掉Beta分布、平均分布相关:

对应的看下svo2.0中关于更新深度滤波算法高斯模型中的平均值 [公式] 、方差 [公式] 。代码如下：

bool updateFilterGaussian(const FloatType z, // Measurementconst FloatType tau2,Eigen::Ref<SeedState>& mu_sigma2_a_b) {FloatType& mu = mu_sigma2_a_b(0);//平均值FloatType& sigma2 = mu_sigma2_a_b(1);//深度方差FloatType& a = mu_sigma2_a_b(2);//Beta分布的a FloatType& b = mu_sigma2_a_b(3);const FloatType norm_scale = std::sqrt(sigma2 + tau2);if(std::isnan(norm_scale)) {LOG(WARNING) << "Update Seed: Sigma2+Tau2 is NaN";return false;}const FloatType denom = (sigma2 + tau2);mu = (sigma2 * z + tau2 * mu) / denom;//新的均值mu  对应公式43，以及14中的msigma2 = sigma2 * tau2 / denom;//对应公式44，以及14中的s2CHECK_GE(sigma2, 0.0);CHECK_GE(mu, 0.0);return true;
}

最后会判断该种子点是否收敛，这部分也值得推敲哦，先看下代码：

 if(seed::isConverged(state,ref_frame.seed_mu_range_,sigma2_convergence_threshold))//如果协方差小于阈值，就认为收敛了，它就不再是种子点，而是candidate点，使用回调函数，加入到候选点队列中{if(ref_ftr.type == FeatureType::kCornerSeed)//这里通过改变其属性类别来 代替删除seed效果，不再进行updateseeds分析ref_ftr.type = FeatureType::kCornerSeedConverged;else if(ref_ftr.type == FeatureType::kEdgeletSeed)ref_ftr.type = FeatureType::kEdgeletSeedConverged;else if(ref_ftr.type == FeatureType::kMapPointSeed)ref_ftr.type = FeatureType::kMapPointSeedConverged;}

其中：

ref_frame.seed_mu_range_ = 1/depth_init_min 即逆深度最大值，

另外sigma2_convergence_threshold是配置值，

thresh = mu_range / sigma2_convergence_threshold，

5 更新地图点

前面利用cur_frame更新了ref_frame的深度滤波分布状态、参数。最后需要利用最新深度计算结果来更新地图点，主要 upgradeSeedsToFeatures()函数中实现。上面已经敲了一万三千字了，上面算法推导确实也是重点，但是这里更新地图点的过程才是深入理解作者使用深度滤波的入口！

上面是离子老师画得简单的示意图，关于upgradeSeedsToFeatures的借鉴下，关于这个函数的逻辑，在当前帧计算深度滤波更新概率，得到新的方差、平均值后：

void FrameHandlerBase::upgradeSeedsToFeatures(const FramePtr &frame) {VLOG(40) << "Upgrade seeds to features";size_t update_count = 0;size_t unconverged_cnt = 0;for (size_t i = 0; i < frame->num_features_; ++i) {if (frame->landmark_vec_[i]) {const FeatureType &type = frame->type_vec_[i];if (type == FeatureType::kCorner || type == FeatureType::kEdgelet ||type == FeatureType::kMapPoint) {frame->landmark_vec_[i]->addObservation(frame, i);} else {CHECK(isFixedLandmark(type));frame->landmark_vec_[i]->addObservation(frame, i);}} else if (frame->seed_ref_vec_[i].keyframe) {if (isUnconvergedSeed(frame->type_vec_[i])) {unconverged_cnt++;}SeedRef &ref = frame->seed_ref_vec_[i];// In multi-camera case, it might be that we already created a 3d-point// for this seed previously when processing another frame from the bundle.PointPtr point = ref.keyframe->landmark_vec_[ref.seed_id];if (point == nullptr) {// That's not the case. Therefore, create a new 3d point.Position xyz_world = ref.keyframe->T_world_cam() *ref.keyframe->getSeedPosInFrame(ref.seed_id);point = std::make_shared<Point>(xyz_world);ref.keyframe->landmark_vec_[ref.seed_id] = point;ref.keyframe->track_id_vec_[ref.seed_id] = point->id();point->addObservation(ref.keyframe, ref.seed_id);}// add reference to current frame.frame->landmark_vec_[i] = point;frame->track_id_vec_[i] = point->id();point->addObservation(frame, i);if (isCorner(ref.keyframe->type_vec_[ref.seed_id])) {ref.keyframe->type_vec_[ref.seed_id] = FeatureType::kCorner;frame->type_vec_[i] = FeatureType::kCorner;} else if (isMapPoint(ref.keyframe->type_vec_[ref.seed_id])) {ref.keyframe->type_vec_[ref.seed_id] = FeatureType::kMapPoint;frame->type_vec_[i] = FeatureType::kMapPoint;} else if (isEdgelet(ref.keyframe->type_vec_[ref.seed_id])) {ref.keyframe->type_vec_[ref.seed_id] = FeatureType::kEdgelet;frame->type_vec_[i] = FeatureType::kEdgelet;// Update the edgelet direction.double angle = feature_detection_utils::getAngleAtPixelUsingHistogram(frame->img_pyr_[frame->level_vec_[i]],(frame->px_vec_.col(i) / (1 << frame->level_vec_[i])).cast<int>(),4u);frame->grad_vec_.col(i) =GradientVector(std::cos(angle), std::sin(angle));} else {CHECK(false) << "Seed-Type not known";}++update_count;}// when using the feature-wrapper, we might copy some old references?frame->seed_ref_vec_[i].keyframe.reset();frame->seed_ref_vec_[i].seed_id = -1;}VLOG(5) << "NEW KEYFRAME: Updated " << update_count<< " seeds to features in reference frame, "<< "including " << unconverged_cnt << " unconverged points.\n";const double ratio = (1.0 * unconverged_cnt) / update_count;if (ratio > 0.2) {LOG(WARNING) << ratio * 100 << "% updated seeds are unconverged.";}
}

遍历当前帧所有特征点，判断是否存在对应的3d地图点，如果存在则为该landmark添加上该cur_frame下该特征的的observation关系
如果不存在对应的3d地图点，但是该特征的共视历史seed所在的帧为关键帧时，不管当前seed所在的滤波结果是否成熟，都为这些ref和cur图像帧利用深度计算一个3d点（利用深度滤波过程计算的更新后的深度mu），并将这些kCorner、kCornerSeed、kCornerCovergedSeed、kEdgelet、kEdgeletSeed、kEdgeletCovergedSeed等转为kCorner、kEdgelet，则历史共视帧上的这些共视seed不再是种子点（如图二中的a、b、c三个seed点），有3d信息后是地图点了（如图二中cur_frame中共视seeds变为unseeds）。后面会在重投影reprojector中计算这些地图点，点数不够时会从uncovergedseed中选一些点来用。此时，当前帧一部分成熟seed点转为kCorner等有3d信息的点，一部分可能没有共视关系的seed会在之后计算用到。

6. 参考资料：

https://guoqiang.blog.csdn.net/article/details/121485389
栗子：白话SVO2.0之深度滤波