SVO学习日记-10--2021.2.7

SVO-10-matcher–2021.2.7

matcher.h

#ifndef SVO_MATCHER_H_
#define SVO_MATCHER_H_

#include <svo/global.h>

namespace vk{
	class AbstractCamera;
	namespace patch_score{
		template<int HALF_PATCH_SIZE> class ZMSSD;
	}
}

namespace svo{
	class Point;
	class Frame;
	class Feature;
	
	namespace warp{
		
		//从参考视角到当前视角的图像块的仿射变换
		void getWarpMatrixAffine(
			const vk::AbstractCamera& cam_ref,//参考视角
			const vk::AbstractCamera& cam_cur,//当前视角
			const Vector2d& px_ref,//参考图像中的像素坐标
			const Vector3d& f_ref,//参考图像的方向向量
			const double depth_ref,//参考帧图像的深度
			const SE3& T_cur_ref,//参考帧到当前帧的变换矩阵
			const int level,//金字塔层
			Matrix2d& A_cur_ref);//仿射变换矩阵
			
		//得到最好的搜索金字塔层	
		int getBestSearchLevel(const Matrix2d& A_cur_ref,//仿射变换矩阵
							   const int max_level);
							   
		//仿射变换
		void warpAffine(const Matrix2d& A_cur_ref,const cv::Mat& img_ref,
					    const Vector2d& px_ref,const int level_ref,
						const int level_cur,const int halfpatch_size,
						uint8_t* patch);
		}
		//重投影匹配中进行块匹配，以及在三角化中进行极线搜索
		class Matcher{
			public:
			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
			
			static const int halfpatch_size_ = 4;
			static const int patch_size_ = 8;
			
			//patch的ZMSSD的得分
			typedef vk::patch_score::ZMSSD<halfpatch_size_> PatchScore;
			
			//一些定义
			struct Options{
				bool align_1d;//在极线搜索过程中，研究极限来对齐Patch
				int align_max_iter;//在GN法中，对齐特征Patch所进行的迭代次数
				double max_epi_length_optim;//跳过极线搜索并直接进行图像对齐的最大极线长度
				size_t max_epi_search_steps;//沿着极线的最大步长
				bool subpix_refinement;//极线搜索后进行GN法特征对齐
				bool epi_search_edgelet_filtering;
				double epi_search_edgelet_max_angle;
				
				Options():
				align_1d(false),
				align_max_iter(10),
				max_epi_length_optim(2.0),
				max_epi_search_steps(1000),
				subpix_refinement(true),
				epi_search_edgelet_filtering(true),
				epi_search_edgelet_max_angle(0.7)
				{}
			} options_;
			
			uint8_t patch_[patch_size_ * patch_size_] __attribute__ ((aligned(16)));//patch
			uint8_t patch_with_border_ [(patch_size_ + 2) * //patch加上边界指针
										(patch_size_ + 2)] __attribute__ ((aligned(16)));
			Matrix2d A_cur_ref;//仿射变换矩阵
			Vector2d epi_dir_;//极线段向量
			double epi_lenght_;//极线搜索中，极线的长度
			double h_inv_;//沿极线进行图像对齐的hessian
			int search_level_;//最佳搜索的金字塔层
			bool reject_;
			Feature* ref_ftr_;//待进行块匹配的特征
			Vector2d px_cur_;//当前帧的匹配特征位置
			
			Matcher() = default;
			~Matcher() = default;
			
			//寻找直接采用refinement的匹配
			//这个函数是已知当前帧的匹配特征有2-3个像素差
			bool findMatchDirect(const Point& pt,
								 const Frame& frame,
								 Vector2d& px_cur);
			
			//沿着极线不使用任何特征进行搜索匹配
			bool findEpipolarMatchDirect(const Frame& ref_frame,
										 const Frame& cur_frame,
										 const Feature& ref_ftr,
										 const double d_estimate,
										 const double d_min,
										 const double d_max,
										 double& depth);
			
			void createPatchFromPatchWithBorder();
		}
}

#endif
								

matcher.cpp

#include <cstdlib>
#include <vikit/abstract_camera.h>
#include <vikit/vision.h>
#include <vikit/math_utils.h>
#include <vikit/patch_score.h>
#include <svo/matcher.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/point.h>
#include <svo/config.h>
#include <svo/feature_alignment.h>

namespace svo{
	namespace warp{
		
		void getWarpMatrixAffine(const vk::AbstractCamera& cam_ref,
							     const vk::AbstractCamera& cam_cur,
								 const Vector2d& px_ref,
								 const Vector3d& f_ref,
								 const double depth_ref,
								 const SE3& T_cur_ref,
								 const int level_ref,
								 Matrix2d& A_cur_ref){
			
			//计算仿射变换矩阵 A_ref_cur
			//计算窗口的思路是先利用三点法计算出tk和tn两时刻图像的仿射变换矩阵，
			//然后再把窗口w1里的像素坐标逐一映射到图像tn里，这样映射的所有坐标就组成了窗口w2。
			const int halfpatch_size = 5;//考虑边界的一半
			const Vector3d& xyz_ref(f_ref * depth_ref);//点在ref下的3D坐标
			
			/*层数越大加的数越大的原因：
			px_ref是在某一层金字塔层上提取的，但是最后需要扩大相应倍数到0层的坐标上，
			因为特征是在level_ref上提取的，所以该层的Patch对应到0层上要扩大到相应倍数
			*/
			
			//图像上根据金字塔层数对应Patch大小，得到patch的右上角坐标（对应层的patch图像坐标）
			Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0) * (1 <<level_ref)));
			//图像上根据金字塔层数对应Patch大小，得到patch的左下角坐标（对应层的patch图像坐标）
			Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size) * (1 < level_ref)));
			
			//根据点的深度值得到右上角、左下角的坐标
			xyz_du_ref *= xyz_ref[2] / xyz_du_ref[2];
			xyz_dv_ref *= xyz_ref[2] / xyz_dv_ref[2];
			
			//将这三点变换到cur下的图像坐标
			const Vector2d px_cur(cam_cur.world2cam(T_cur_ref * (xyz_ref)));//变换到像素坐标
			const Vector2d px_du(cam_cur.world2cam(T_cur_ref * (xyz_du_ref)));//以及对应点像素坐标
			const Vector2d px_dv(cam_cur.world2cam(T_cur_ref * (xyz_dv_ref)));
			
			//把原来的当作轴，变换得到对应的轴就是两列
			//ref到cur第0层的变换
			A_cur_ref.col(0) = (px_du - px_cur) / halfpatch_size;
			A_cur_ref.col(1) = (px_dv - px_cur) / halfpatch_size;
		}
		
		//找到合适的搜索的匹配金字塔层（从cur帧上找），现在是在第0层
		//当前帧的第0层，获取参考帧的最佳层
		int getBestSearchLevel(const Matrix2d& A_cur_ref,
							   const int max_level){
			int search_level = 0;
			double D = A_cur_ref.determinant();	//计算仿射矩阵的行列式
			while(D > 3.0 && search_level <max_level){//A值小于3，且没达到最高层
				search_level += 1;
				D *= 0.25;//每增加一层行列式的值除以4
				return search_level;//返回具体是在哪一层
			}
		}
		
		/*将ref上的图像块通过A_cur_ref变换到Patch
		  img_ref:变换前的ref对应金字塔层数的图像
		  px_ref:ref上0层特征点的位置
		  patch:patch_with_border_的头指针
		*/
		void warpAffine(const Matrix2d& A_cur_ref,
						const cv::Mat& img_ref,
						const Vector2d& px_ref,
						const int level_ref,
						const int search_level,
						const int halfpatch_size,
						uint8_t* patch){
			const int patch_size = halfpatch_size *2;
			
			//求逆相当于从cur第0层到ref第level_ref层变换
			const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
			if(isnan(A_ref_cur(0,0))){
				printf("Affine warp is NaN, probably camera has no translation\n"); // TODO
				return;
			}
			//头指针
			uint8_t* patch_ptr = patch;
			
			//0层最大，越往上越小
			//变换到ref对应的层上
			const Vector2f px_ref_pyr = px_ref.cast<float>() / (1 <<level_ref);
			
			//遍历窗口
			for(int y = 0;y < patch_size;++y){
				for(int x = 0; x < patch_size;++x，++patch_ptr){
					
					//以中点建立的坐标系，得到的px_patch
					Vector2f px_patch(x - halfpatch_size,y - halfpatch_size);
					
					//search_level是相当于cur的层，变换到第0层进行仿射变换
					px_patch *= (1 <<search_level);//在相应的层搜索
					
					//仿射变换到ref上
					const Vector2f px(A_ref_cur * px_patch + px_ref_pyr);
					if(px[0]<0 || px[1]<0 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1)
						*patch_ptr = 0;
					else
						*patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref,px[0],px[1]);
				}
			}
		}
		
		/*已知参考帧和当前帧的位姿，进行块匹配，最终计算ref上精确的深度
		cur_frame:匹配的当前帧
		d_estimate:估计的深度值
		depth:最终求得的ref上特征点的深度
		*/
		bool Matcher::findEpipolarMatchDirect(const Frame& ref_frame,
											  const Frame& cur_frame,
											  const Feature& ref_ftr,
											  const double d_estimate,
											  const double d_min,
											  const double d_max,
											  double& depth){
			//从参考帧到当前帧的变换矩阵（到当前帧的归一化坐标系）									  
			SE3 T_cur_ref = cur_frame.T_f_w_ * ref_frame.T_f_w_.inverse();
			int zmssd_best = PatchScore::threshold();
			Vector2d uv_best;
			
			//得到当前帧单位平面上的极线范围
			Vector2d A = vk::project2d(T_cur_ref * (ref_ftr.f * d_min));//最小深度对应的极线端点
			Vector2d B = vk::project2d(T_cur_ref * (ref_ftr.f * d_max));//最大深度对应的极线端点
			epi_dir_ = A - B;//单位平面上极线段向量
			
			//首先计算一个粗略的仿射矩阵
			warp::getWarpMatrixAffine(*ref_frame.cam_,*cur_frame.cam_,ref_ftr.px,
									  ref_ftr.f,d_estimate,T_cur_ref,ref_ftr.level,A_cur_ref);
			
			reject = false;
			
			//这里是边界特征预判断，现在已经没有了
			if(ref_ftr.type == Feature::EDGELET && options_.epi_search_edgelet_filtering){
				const Vector2d grad_cur = (A_cur_ref * ref_ftr.grad).normalized();
				const double cosangle = fabs(grad_cur.dot(epi_dir_.normalized()));
				if(cosangle < options_.epi_search_edgelet_max_angle){
					regect_ = true;
					return false;
				}
			}
			
			//找到在当前帧上最佳的搜索金字塔层，以及极线的搜索范围
			search_level_ = warp::getBestSearchLevel(A_cur_ref_,Config::nPyrLevels() - 1);
			
			//从单位平面上投影到像素平面
			Vector2d px_A(cur_frame.cam_ -> world2cam(A));
			Vector2d px_B(cur_frame.cam_ -> world2cam(B));
			epi_length_ = (px_A - px_B).norm() / (1 << search_level_);//计算对应层的极线长度
			
			//将参考帧上的Patch变换到当前帧上，并去掉边界border
			warp::warpAffine(A_cur_ref_,ref_frame.img_pyr_[ref_ftr.level],ref_ftr.px,ref_ftr.level,
							 search_level_,halfpatch_size_ + 1,patch_with_border_);
			
			createPatchFromPatchWithBorder();
			
			//若极线长度小于2，则进行特征对齐得到更精确的特征点位置
			//然后进行三角化计算深度
			//极线长度小于2时，则计算其附近的八个点
			if(epi_length_ <2.0){
				px_cur_ = (px_A + px_B) / 2.0;//取平均值
				Vector2d px_scaled(px_cur_/(1 <<search_level_));//将当前帧的该点变换到相应层
				bool res;
				//特征对齐
				if(options_.alignment::align_1d){
					res = feature_alignment::align1D(cur_frame.img_pyr_[search_level_], 
													(px_A-px_B).cast<float>().normalized(),
													patch_with_border_, patch_, options_.align_max_iter, 
													px_scaled, h_inv_);
				}else{
					res = feature_alignment::align2D(cur_frame.img_pyr_[search_level_], 
													 patch_with_border_, patch_,
													 options_.align_max_iter, px_scaled);
				}
				if(res){
					px_cur_ = px_scaled * (1 <<search_level_);
					
					//计算深度
					if(depthFromTrianfulation(T_cur_ref,ref_ftr.f,cur_frame.cam_ -> cam2world(px_cur_),depth));
					{
						return true;
					}
					retru false;
				}
				
				size_t n_steps = epi_length_ / 0.7;	//步数
				Vector2d step = epi_dir_ / n_steps;//步长
				
				//如果步数大于最大步数
				if(n_steps > options_.max_epi_search_steps){
					printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n",
							n_steps, epi_length_, d_min, d_max);
					return false;
				}
					
				//进行匹配前预计算一些值
				int pixel_sum = 0;
				int pixel_sum_square = 0;
				PatchScore patch_score(patch_);
				
				//在之前先求出单位平面极线段上进行搜索ZMSSD得分最小的patch
				//对B每增加一个step进行搜索，下面是步进式递进
				Vector2d uv = B - step;
				Vector2i last_checked_pxi(0,0);
				++n_steps;//上面先减了一个，多加一步
				
				//开始遍历
				for(size_t i = 0;i < n_steps;++i;uv += step){
					Vector2d px(cur_frame.cam_ -> world2cam(uv));//转换到图像坐标系下
					Vector2i pxi(px[0] / 1 << search_level_) + 0.5,
								 px[1] / 1 << search_level_) + 0.5;//主要是为了到对应的层上
					if(pxi == last_checked_pxi)continue;
					last_checked_pxi = pxi;
					
					//检查patch是否都在内部
					if(!cur_frame.cam_ -> isInFrame(pxi,patch_size_,search_level_)) continue;
					
					//获取cur_patch_ptr的头指针
					uint8_t* cur_patch_ptr = cur_frame.img_pyr_[search_level_].data
										   + (pxi[1] - halfpatch_size_) * cur_frame.img_pyr_[search_level_].cols
										   + (pxi[0] - halfpatch_size_);
					
					//计算极线上patch与ref仿射变换得到的patch之间的分数
					int zmssd = patch_score.computeScore(cur_patch_ptr,cur_frame.img_pyr_[search_level_.cols]);
					
					if(zmssd < zmssd_best){
						zmssd_best = zmssd;
						uv_best = uv;
					}
				}
				
				//满足条件则进行精确的特征对齐，并进行三角化，得到ref上的深度
				if(zmssd_best < PatchScore::threshold()){
					if(options_.subpix_refinement){//若进行精确优化的话
						px_cur_ = cur_frame.cam_ -> world2cam(uv_best);//在把单位平面上的点转换到像素平面
							Vector2d px_scaled(px_cur_/(1 <<search_level_));//在变换到对应层
							bool res;
							if(options_.align_1d)
								res = feature_alignment::align1D(
									  cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
									  patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
							else
								res = feature_alignment::align2D(
									  cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
									  options_.align_max_iter, px_scaled);
							if(res)
							{
								px_cur_ = px_scaled*(1<<search_level_);
								if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
									return true;
							}
							return false;
					}
					
					//若设置不进行精确优化，则用最小得分进行三角化求得深度
					px_cur_ = cur_frame.cam_ ->world2cam(uv_best);
					if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth))
						return true;
				}
				return false;
			}
		}
		
		//三角化
		//返回的是ref下的深度
		bool depthFromTriangulation(const SE3& T_search_ref,
									const Vector3d& f_ref,
									const Vector3d& f_cur,
									double& depth){
			
			//A是[RX1,X2]
			Matrix<double,3,2> A; A << T_search_ref.rotation_matrix() * f_ref,f_cur;
			const Matrix2d AtA = A.transpose() * A;
			
			//判断是否可逆
			if(AtA.determinant() <0.000001) return false;
			
			//求解正规方程
			//d = -(A^T * A).inverse() * A^T * t
			const Vector2d depth2 = -AtA.inverse() * A.transpose() * T_search_ref.translation();
			depth = fabs(depth2[0]);
			return true;
		}
		
		void Matcher::createPatchFromPatchWithBorder(){
			uint8_t* ref_patch_ptr = patch_;
			for(int y = 1;y < patch_size_+1;++y.ref_patch_ptr += patch_size_){
				uint8_t* ref_patch_border_ptr = patch_with_border_ + y*(patch_size_+2) + 1; // patch_with_border移动到第y+1行, 并且由于border多加一
				for(int x=0; x<patch_size_; ++x)
					ref_patch_ptr[x] = ref_patch_border_ptr[x];
			}
		}
		
		//直接使用图像对齐来进行匹配
		//pt:匹配特征对应的3D点，px_cur:当前匹配的特征位置，输出
		bool Matcher::findMatchDirect(const Point& pt,const Frame& cur_frame,Vector2d& px_cur){
			
			//找到与pt对应的，离当前帧最近的帧上的特征
			if(!pt.getCloseViewObs(cur_frame.pos(),ref_ftr_)) return false;
			
			//验证该特征patch是否超过该层图像的大小
			if(!ref_ftr_ -> frame -> cam_ -> isInFrame(ref_ftr_ -> px.cast<int>()/(1<<ref_ftr_-> level),
			   halfpatch_size_+2,ref_ftr_ -> level)) return false;
			   
			//根据ref_ftr_周围的patch求得ref到cur之间的仿射矩阵
			warp::getWarpMatrixAffine(*ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f,
									//! 深度就是这样定义的，对单位平面坐标进行了归一化
									//* 这里深度本就是不准确的,为了求深度才进行的匹配
										(ref_ftr_->frame->pos() - pt.pos_).norm(), 
										cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse(), ref_ftr_->level, A_cur_ref_);
			
			//找到当前帧最合适搜索的金字塔层
			search_level_ = warp::getBestSearchLevel(A_cur_ref_,Config::nPyrLevels() - 1);
			
			//利用上述仿射变换将ref变换到patch,得到搜索层上对应的patch
			warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
							ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_);
			//去掉patch_with_border的边界
			createPatchFromPatchWithBorder();
			
			//缩小到cur对应的层数
			Vector2d px_scaled(px_cur / (1 <<search_level_));
			
			//使用逆向组合图像对齐，得到优化后的px_scaled
			bool success = false;
			if(ref_ftr_ -> type == Feature::EDGELET){
				Vector2d dir_cur(A_cur_ref_ * ref_ftr_ -> grad);
				dir_cur.normalize();
				success = feature_alignment::align1D(
							cur_frame.img_pyr_[search_level_],dir_cur.cast<float>(),
							patch_with_border_,patch_,options_.align_max_iter,px_scaled,h_inv_);
			}else{
					success = feature_alignment::align2D(
							cur_frame.img_pyr_[search_level_],patch_with_border_,patch_,
							options_.align_max_iter,px_scaled);
			}
			px_cur = px_scaled*(1 << search_level_);//扩大到相应的倍数，到第0层
		}
	}
}