SVO学习日记-11--2021.2.11

SVO-11-depth_filter–2021.2.11

depth_filter.h

#ifndef SVO_DEPTH_FILTER_H_
#define SVO_DEPTH_FILTER_H_

#include <queue>
#include <boost/thread.hpp>
#include <boost/function.hpp>
#include <vikit/performance_monitor.h>
#include <svo/global.h>
#include <svo/feature_detection.h>
#include <svo/matcher.h>

namespace svo{
	class Frame;
	class Feature;
	class Point;
	
	//seed表示：对于单一像素的概率深度估计
	struct Seed{
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
		
		static int batch_counter;//每初始化一次(增加关键帧)就一个batch
		static int seed_counter;//总的种子点数量
		int batch_id;//每当种子被创建的时候，关键帧的batch id
		int id;//种子id,仅用于观测
		int live_time;//种子存活在多少个帧中
		Feature* ftr;//关键帧中深度应该被计算的特征
		float a;//Beta分布，越高，内点的概率越大
		float b;//Beta分布，越高，外点的概率越大
		float mu;//正态分布的均值
		float z_range;//可能深度的最大范围
		float sigma2;//正态分布的方差
		Matrix2d patch_cov;//参考图像中，patch的协方差
		
		//每次构造就增加seed_counter
		Seed(Feature* ftr,float depth_mean,float depth_min);
		~Seed();
	}
	
	class DepthFilter{
		public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
		
		typedef boost::unique_lock<boost::mutex> lock_t;//线程锁
		
		//回调函数，为了将收敛的点传给point_candidate_
		typedef boost::function<void (Point*,double)> callback_t;
		
		struct Options{
			bool check_ftr_angle;//如果极线与梯度正交的话，梯度特征才会被更新
			bool epi_search_id;//限制从在极线上搜索的GN法
			bool verbose;//显示输出
			bool use_photometric_disparity_error;//是否使用光度误差来代替1Px的像素误差
			int max_n_kfs;//维持种子的最大关键帧数目
			double sigma_i_sq;//图像噪声
			double seed_convergence_sigma2_thresh;//对于收敛过程中深度不确定性的阈值
			Options():
			check_ftr_angle(false),
			epi_search_id(false),
			verbose(false),
			use_photometric_disparity_error(false),
			max_n_kfs(3),
			sigma_i_sq(5e-4),
			seed_convergence_sigma2_thresh(200.0){}
		} options_;
		
		//深度滤波类的构造函数
		DepthFilter(feature_detection::DetectorPtr feature_detector,callback_t seed_converged_cb);
		
		virtual ~DepthFilter();
		void startThread();//当种子的根心应该在并行线程中进行时，开始线程
		void stopThread();//停止正在运行的线程
		void addFrame(FramePtr frame);//添加一个普通帧到被处理的双端队列里
		void addKeyframe(FramePtr frame,double depth_mean,double depth_min);//添加一个关键帧到双端队列里
		
		//从特定关键帧中去除被初始化的种子，当一个帧从地图中去除时，这个函数表示确保没有种子指向不存在的帧
		void removeKeyframe(FramePtr frame);
		void reset();//重置
		
		//返回属于该帧种子的复制副本，保证线程安全
		//被用于并行计算下一个最好的视角，确保是有效的指向帧的指针，
		//同时确保不会被删除
		void getSeedsCopy(const FramePtr& frame,std::list<Seed>& seeds);
		std::list<Seed>& getSeeds() {return seeds_;}//返回种子的引用
		
		//种子的bayes更新
		//参数：测量值，不确定性，种子
		static void updateSeed(const float x,const float tau2,Seed* seed);
		
		//计算测量值的不确定性
		static double computeTau(const SE3& T_ref_cur,const Vector3d& f,const double z,const double px_error_angle);
		
		std::ofstream f;
		
		protected:
		feature_detection::DetectorPtr feature_detector_;//检测特征点
		callback_t seed_converged_cb_;//回调函数
		std::list<Seed> seeds_;//种子点的列表
		boost::mutex seeds_mut_;//更新种子点的线程锁
		bool seeds_updating_halt_;//当种子更新时应该被打断的时候，设置为true
		boost::thread* thread_;//子线程
		std::queue<FramePtr> frame_quene_;//frame的队列
		boost::mutex frame_queue_mut_;//增加frame到队列的线程锁
		boost::condition_variable frame_queue_cond_;//条件变量
		FramePtr new_keyframe_;//需要提取新种子的新的关键帧
		bool new_keyframe_set_;//是否需要处理新关键帧
		double new_keyframe_min_depth_;//新关键帧中的最小深度，用于构建新种子的深度范围
		double new_keyframe_mean_depth_;//深度中值
		vk::PerformanceMonitor permon_;//性能检测
		Matcher matcher_;
		
		//从一个帧中初始化新的种子
		void initializedSeeds(FramePtr frame);
		virtual void updateSeeds(FramePtr frame);//更新所有具有新测量值的种子
		void clearFrameQueue();//当一个新的帧进来，清除frame queue
		void updateSeedsLoop();//一个连续更新种子的线程
	};
}

#endif

			

depth_filter.cpp

#include <algorithm>
#include <vikit/math_utils.h>
#include <vikit/abstract_camera.h>
#include <vikit/vision.h>
#include <boost/bind.hpp>
#include <boost/math/distributions/normal.hpp>
#include <svo/global.h>
#include <svo/depth_filter.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/feature.h>
#include <svo/matcher.h>
#include <svo/config.h>
#include <svo/feature_detection.h>

namespace svo{
	int Seed::batch_counter = 0;
	int Seed::seed_counter = 0;
	
	Seed::Seed(Feature* ftr,float depth_mean,float depth_min):
		batch_id(batch_counter),//创建该种子的关键帧ID
		id(seed_counter++),//用来可视化
		ftr(ftr),//特征点，需要计算深度的特征点
		a(10),
		b(10),
		mu(1.0 / depth_mean),//正态分布的初始均值，设置为平均深度的倒数
		z_range(1.0 / depth_mean),//最大逆深度
		sigma2(z_range * z_range / 36)//参考图像中的patch协方差
		{}
	
		/*深度滤波器的初始化
		///输入： feature	feature_detector
		///		 seed_converged_cb	一个函数指针，在此之前，初始化函数中已经将这个回调函数
		//							绑定到地图中候选关键点的成员函数newCandidatePoint()中了
		*/
	DepthFilter:;DepthFilter(feature_detection::DetectorPtr feature_detector,callback_t seed_converged_cb):
		feature_detector_(feature_detector),//检测特征点
		seed_converged_cb_(seed_converged_cb),//回调函数
		seeds_updating_halt_(false),//此时种子更新不应该被打断
		thread_(NULL),//子线程
		new_keyframe_set_(false),//此时不需要处理新的关键帧
		new_keyframe_min_depth_(0.0),//新关键帧中的最小深度
		new_keyframe_mean_depth_(0.0)//深度中值
		{}
		
	DepthFilter::DepthFilter(){
		stopThread();
		SVO_INFO_STREAM("DepthFilter destroted");
	}
	
	//启动深度滤波线程
	void DepthFilter::startTread(){
		thread_ = new boost::thread(&DepthFilter::updateSeedsLoop,this);//种子更新线程
	}
	
	//线程终止
	void DepthFilter::stopThread(){
		SVO_INFO_STREAM("DepthFilter stop thread invoked.");
		if(thread_ != NULL){
			SVO_INFO_STREAM("DepthFilter interrupt and join thread...");
			seeds_updating_halt_ = true;//种子点暂停更新
			thread_ -> interrupt();//线程暂停
			thread_ ->join();//等待子线程执行完成返回
			thread_ = NULL;//删除线程
		}
	}
	
	/*普通帧的深度滤波函数，实际上在一般情况下，深度滤波线程一直开着，thread_一般都不为NULL,
	因此一般都能执行if，并不会执行else,后台还运行一个深度滤波线程，可能阻塞但不是NULL，一直
	在等待帧队列有帧插入，一旦有帧插入，notify_one()就会唤醒深度滤波线程，深度滤波线程中绑定了
	updateSeedsLoop()函数，同样会调用updateSeeds()函数进行种子深度滤波，常规情况下由专门的深度滤波
	线程来处理深度滤波，而并非采用主线程
	*/
	void DepthFilter::addFrame(FramePtr frame){
		if(thread_ != NULL){
			{
				lock_t lock(frame_queue_mut_);//线程锁
				if(frame_queue_.size() >2) frame_queue_.pop();//超过两帧就丢弃一帧，提高速度
				frame_queue_.push(frame);//放入队尾
			}
			seeds_updating_halt_ = false;
			frame_queue_cond_.notify_one();//唤醒深度滤波线程，wait的线程
		}
		else{
			updateSeeds(frame);//更新所有具有新测量值的种子（不使用多线程的时候更新）
		}
	}
	
	/*对于关键帧，不需要添加到帧的队列，标记为关键帧即可，然后后台深度滤波线程开启，去处理最新的
	关键帧，主线程调用添加新的关键帧函数，用专门的深度滤波线程进行种子滤波
	*/
	void DepthFilter::addKeyframe(FramePtr frame,double depth_mean,double depth_min){
		new_keyframe_min_depth_ = depth_min;
		new_keyframe_mean_depth_ = depth_mean;
		if(thread_ != NULL){
			new_keyframe_ = frame;
			new_keyframe_set_ = true;//此时处理新进来的关键帧
			seeds_updating_halt_ = true;//种子点暂停更新
			frame_queue_cond_.notify_one();//等待，唤醒深度滤波线程，wait的线程
		}else{
			initializeSeeds(frame);
		}
	}
	
	//初始化种子点
	//参数时关键帧的共享指针
	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);
		
		seeds_updating_halt_ = true;//暂停更新种子
		lock_t lock(seeds_mut_);//线程上锁
		++Seed::batch_counter;
		//增加种子点到列表，种子点都是新提取的点
		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)
			SVO_INFO_STREAM("DepthFilter: Initialized " << new_features.size() << " new seeds");
		seeds_updating_halt_ = false;//继续更新种子点
	}
	
	//当关键帧被移除时，对应的种子点也要被移除
	void DepthFilter::removeKeyframe(FramePtr frame){
		seeds_updating_halt_ = true;//暂停更新
		lock_t lock(seeds_mut_);//线程上锁
		std::list<Seed>::iterator it = seeds_.begin();//迭代器，读取上述从关键帧中提取的种子
		size_t n_removed = 0;
		while(it != seeds_.end()){
			if(it -> ftr ->frame == frame.get()){//遍历
				if(!f.is_open()){
					std::cout <<"!!!!!!" << std::endl;
				}
				f << it -> id <<", " <<it ->live_time <<", 0" <<endl;
				it = seeds_.erase(it);//清除
				++n_removed;//清除数叠加
			}else{
				++it;//继续迭代器
			}
		}
		seeds_updating_halt_ = false;//继续更新种子点
	}
	
	//重置深度滤波器，清空seed frame
	void DepthFilter::reset(){
		seeds_updating_halt_ = true;//暂停更新
		{
			lock_t lock(seeds_mut_);//上锁
			seeds_.clear();//清除seeds
		}
		
		lock_t lock(frame_queue_mut_);//上锁帧
		while(!frame_queue_.empty()){//若不为空
			frame_queue_.pop();//则从第一个开始删除
		}
		seeds_updating_halt_ = false;//继续
		
		if(options_.verbose)
			SVO_INFO_STREAM("DepthFilter:reset");
	}
	
	//更新种子点的循环
	//更新大回环，updateSeeds的前置，等待线程在这里处理
	void DepthFilter::updateSeedsLoop(){
		
		//当有请求时，终止
		while(!boost::this_thread::interrpution_requested()){
			FramePtr frame;
			{
				lock_t lock(frame_queue_mut_);//检测是否有线程锁
				
				//当帧队列是空的，且不需要处理关键帧的时候
				while(frame_queue_.empty() && new_keyframe_set_ == false)
					frame_queue_cond_.wait(lock);//线程等待
				
				//若是新的关键帧，则重新处理
				if(new_keyframe_set_){
					new_keyframe_set_ = false;
					seeds_updating_halt_ = false;//继续
					clearFrameQueue();//清除普通帧队列
					frame = new_keyframe_;//把新的关键帧赋值给frame
				}else{//若不是新的关键帧
					frame = frame_queue_.front();//则用普通帧的第一个赋值给frame
					frame_queue_.pop();//然后从队列里再把第一个给删除
				}
			}
			//利用最新的帧来更新地图，在上面赋值给了frame
			updateSeeds(frame);
			if(frame -> isKeyframe())//同时如果是关键帧的话
				initializeSeeds(frame);//初始化种子点
		}
	}
	
	//对所有的种子进行更新（包括逆深度和不确定性）
	void DepthFilter::updateSeeds(FramePtr frame){
		
		//为了保证速度，只能更新一定数量的种子
		size_t n_updates = 0,n_failed_matches = 0,n_seeds = seeds_.size();
		lock_t lock(seeds_mut_);//上锁
		list<Seed>::iterator it = seeds_.begin();//获取种子列表中的第一颗种子
		
		const double focal_length = frame -> cam_ -> errorMultiplier2();//获取相机的焦距
		double px_noise = 1.0;//像点位置的噪声
		
		//这里这一步要阅读REMODE论文中公式16或者视觉SLAM十四讲中深度滤波那一部分
		double px_error_angle = atan(px_noise / (2.0*focal_length)) * 2.0;
		
		while(it != seeds_.end()){
			
			//此时种子滤波暂停
			//当新的关键帧插入的时候，暂停种子滤波，之后再次开启
			if(seeds_updating_halt_) return;
			
			//有些种子无论更新多少次也不收敛，从队列中删除
			//剔除存在太久的种子
			if((Seed::batch_counter - it -> batch_id) > options_.max_n_kfs){
				it = seeds_.erase(it);
				continue;
			}
			
			//前一半指的是创建当前种子点的关键帧，后一半是指当前帧，可能是frame,kF
			//计算的是从当前帧到参考帧的变换矩阵
			SE3 T_ref_cur = it -> ftr -> frame -> T_f_w_ * frame ->T_f_w_.inverse();
			
			/*mu是种子的平均逆深度，正态分布的均值，f是特征点视线的方向向量
			也就是该特征点视线的方向向量，归一化求得。后面括号里求得是深度乘以向量=特征点的3D坐标
			得到从参考帧到当前帧的3D坐标，这里种子点指的是创建该种子的关键帧坐标
			*/
			const Vector3d xyz_f(T_ref_cur.inverse() * (1.0 / it -> mu * it -> ftr ->f));
			
			//验证
			if(xyz_f.z() < 0.0){
				++it;
				continue;
			}
			
			//检验该种子投影到当前帧中是否落入当前帧的图像区域内，f2c先归一化，再得到像素坐标
			if(!frame ->cam_->isInFrame(frame->f2c(xyz_f).cast<int>())){
				++it;
				continue;
			}
			
			/*  it -> mu:是种子上一次更新后的后验参数，平均逆深度，现在要求当前更新的新参数
				sigma2是patch在参考图像里的协方差
				it -> sigma2:是种子在上一次更新后的后验参数，平均逆深度的方差（不确定性）现在要求当前更新的新参数
				根据上一次估计的后延平均逆深度以及逆深度的不确定性，对当前帧来说就是先验信息了，求最大最小逆深度
				当前帧中得到的估计值梦就在最大最小逆深度范围内
			*/
			float z_inv_min = it -> mu + sqrt(it -> sigma2);//最小深度，在当前帧中最大的逆深度
			float z_inv_max = max(it -> mu - sqrt(it -> sigma2),0.00000001f);//最大深度，在当前帧中最小的逆深度
			
			//z是视线长度，乘以方向向量得到3D坐标
			//新的测量值与上一次的后验平均深度，得到当前帧新的后验参数
			double z;//新的深度测量值
			
			//新的深度值z通过极线搜索来完成，得到了精确深度
			if(!matcher_.findEpipolarMatchDirect(*it->ftr-> frame,*frame,*it->ftr,1.0/it->mu,
				1.o/z_inv_min,1.0/z_inv_max,z)){
				it ->b++;//外点的标记增加
				++it;//继续遍历下一个种子点
				++n_failed_matches;//失败次数+1
				continue;
			}
			
			//上面计算的是深度，这里计算的是深度方差的平方根，标准差
			double tau = computeTau(T_ref_cur,it it -> ftr -> f,z,px_error_angle);
			
			//由上面得到最小最大的逆深度，相减得到最大最小逆深度之差
			double tau_inverse = 0.5 * (1.0/max(0.0000001,z - tau) - 1.0 /(z + tau));
			
			updateSeed(1. / z,tau_inverse * tau_inverse,&*it);
			++n_updates;
			
			//若当前帧是关键帧的话，标记已经生成了新的点，提取点以便扩增种子
			if(frame -> isKeyframe()){
				feature_detector_ ->setGridOccpancy(matcher_.px_cur_);
			}
			
			/*如果种子点已经收敛了，初始化一个新的候选点并且从列表里取出
			z_range:最大深度，it -> sigma2:种子逆深度的方差
			*/
			if(sqrt(it ->sigma2) < it -> z_range / options_.seed_convergence_sigma2_thresh){
				assert(it -> ftr ->point == NULL);
				
				//像素点对应新的世界坐标，it->mu是新的逆深度均值了
				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;
				
				{
					//将地图点point插入到候选地图点candidates_列表中，其中回调函数
					//已经被绑定到初始化函数的成员函数中了
					seed_converged_cb(point,it -> sigma2);
				}
				//对于收敛的点，从种子列表中删除，不在迭代
				it = seeds_.erase(it);
			}
			
			//如果逆深度太大，发散，删除
			else if(isnan(z_inv_min)){
				SVO_INFO_STREAM("z_Min is nan");
				it = seeds_.erase(it);
			}
			else//还需要迭代，还未收敛
				++it;
		}
	}
	
	//清除帧队列
	void DepthFilter::clearFrameQueue(){
		while(!frame_queue_.empty())
			frame_queue_.pop();
	}
	
	//从种子点中找出在frame上的点，并赋值给seeds,关联
	void DepthFilter::getSeedsCopy(const FramePtr& frame,std::list<Seed>& seeds){
		lock_t lock(seeds_mut_);
		for(std::list<Seed>::iterator it = seeds_.begin();it != seeds_.end();++it){
			if(it -> ftr -> frame == frame.get())
				seeds.push_back(*it);
		}
	}
		
	void DepthFilter::updateSeed(const float x,const tau2,Seed* seed){
		float norm_scale = sqrt(seed -> sigma2 +tau2);
		if(std::isnan(norm_scale)) return;
		
		boost::math::normal_distribution<float> nd(seed -> mu,norm_scale);
		
		float s2 = 1. / (1. / seed -> sigma2 +1. / tau2);
		float m = s2 * (seed -> mu / seed ->sigma2 + x / tau2);
		
		float C1 = seed -> a / (seed -> a + seed -> b) * boost::math::pdf(nd,x);
		float C2 = seed -> b / (seed -> a + seed -> b) * 1. / seed -> z_range;
		
		//归一化
		float normalization_constant = C1 + C2;
		C1 /= normalization_constant;
		C2 /= normalization_constant;
		
		//归一化求解f,e.用来更新Beta的参数a,b
		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));
		
		//新的深度d
		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;
		//更新beta分布的a,b的参数
		seed -> a = (e - f) / (f - e / f);
		seed -> b = seed -> a * (1.0f - f) / f;
	}
	
	//计算最新深度测量值的不确定性（标准差），这里是深度而不是逆深度
	//参考REMODE
	//输出：tau 	方差的平方根
	double DepthFilter::computeTau(const SE3& T_ref_cur,const Vector3d& f,
									const double z, const double px_error_angle){
		Vector3d t(T_ref_cur.translation());//两帧之间的平移，基线长度向量
		Vector3d a = f * z - t;//向量a的计算公式
		double t_norm = t.norm();//平移的长度，模
		double a_norm = a.norm();//a的模长
		double alpha = acos(f.dot(t) / t_norm);//参考帧夹角的反余旋值
		double beta = acos(a.dot(-t) / (t_norm * a_norm));//当前帧夹角的反余旋值
		double beta_plus = beta + px_error_angle;//扰动后的beta
		double gamma_plus = PI - alpha - beta_plus;//空间中的点，依据beta_plus的变化而变换
		double z_plus = t_norm * sin(beta_plus) / sin(gamma_plus);//正旋定理求得的深度。从参考帧中看到的
		return (z_plus - z);//扰动后两个深度之差
	}
}