SVO 特征对齐代码分析
SVO稀疏图像对齐之后使用特征对齐，即通过地图向当前帧投影，并使用逆向组合光流以稀疏图像对齐的结果为初始值，得到更精确的特征位置。
主要涉及文件：
reprojector.cpp
matcher.cpp
feature_alignment.cpp
point.cpp
map.cpp
1.入口函数：
void Reprojector::reprojectMap( FramePtr frame, std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs) { resetGrid(); // Identify those Keyframes which share a common field of view. SVO_START_TIMER("reproject_kfs"); //计算当前地图中的关键帧与当前帧frame具有共视关系的关键帧，并返回两帧之间的距离； list< pair<FramePtr,double> > close_kfs; map_.getCloseKeyframes(frame, close_kfs); //按照距离进行排序； // Sort KFs with overlap according to their closeness close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) < boost::bind(&std::pair<FramePtr, double>::second, _2)); // Reproject all mappoints of the closest N kfs with overlap. We only store // in which grid cell the points fall. size_t n = 0; overlap_kfs.reserve(options_.max_n_kfs); //关键帧共视关系数量限制； for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end(); it_frame!=ite_frame && n<options_.max_n_kfs; ++it_frame, ++n) { FramePtr ref_frame = it_frame->first; ref_frame->debug_img_ = ref_frame->img().clone(); //every reproject iteration, the debug_img_ should be reinition overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0)); // Try to reproject each mappoint that the other KF observes for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); it_ftr!=ite_ftr; ++it_ftr) { // check if the feature has a mappoint assigned if((*it_ftr)->point == NULL) continue; //判断该特征是否已经投影过了 // make sure we project a point only once if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) continue; (*it_ftr)->point->last_projected_kf_id_ = frame->id_; //如果参考帧的一个特征在当前帧中投影成功，计数+1； if(reprojectPoint(frame, (*it_ftr)->point)) overlap_kfs.back().second++; } } SVO_STOP_TIMER("reproject_kfs");// Now project all point candidates //投影候选点； SVO_START_TIMER("reproject_candidates"); { boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_); auto it=map_.point_candidates_.candidates_.begin(); while(it!=map_.point_candidates_.candidates_.end()) { if(!reprojectPoint(frame, it->first)) { //候选点如果一直都投影不上，说明这个点可能有问题，可以把它删除； it->first->n_failed_reproj_ += 3; if(it->first->n_failed_reproj_ > 30) { map_.point_candidates_.deleteCandidate(*it); it = map_.point_candidates_.candidates_.erase(it); continue; } } ++it; } } // unlock the mutex when out of scope SVO_STOP_TIMER("reproject_candidates"); // Now we go through each grid cell and select one point to match. // At the end, we should have at maximum one reprojected point per cell. SVO_START_TIMER("feature_align"); for(size_t i=0; i<grid_.cells.size(); ++i) { // we prefer good quality points over unkown quality (more likely to match) // and unknown quality over candidates (position not optimized) //随机的投影每个cell，每投影成功一次+1； if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame)) ++n_matches_; if(n_matches_ > (size_t) Config::maxFts()) break; } SVO_STOP_TIMER("feature_align"); }
2. 计算获得和frame具有共视关系的帧，并返回这些帧和与frame的距离：
//计算frame和当前地图关键帧中具有共视关系的帧，并计算两帧之间的距离； void Map::getCloseKeyframes( const FramePtr& frame, std::list< std::pair<FramePtr,double> >& close_kfs) const { //遍历所有关键帧； for(auto kf : keyframes_) { //遍历关键帧中的特征点，计算是否具有共视关系； // check if kf has overlaping field of view with frame, use therefore KeyPoints for(auto keypoint : kf->key_pts_) { if(keypoint == nullptr) continue; //如果有共视关系，则记录该关键帧和当前帧与该关键帧的距离； if(frame->isVisible(keypoint->point->pos_)) { close_kfs.push_back( std::make_pair( kf, (frame->T_f_w_.translation()-kf->T_f_w_.translation()).norm())); break; // this keyframe has an overlapping field of view -> add to close_kfs } } } }
3.把点投影到当前帧中对应的cell中：
bool Reprojector::reprojectPoint(FramePtr frame, Point* point) { //世界坐标，变换到当前帧下，然后投影到像素坐标； Vector2d px(frame->w2c(point->pos_)); //判断8*8的patch是否在帧内 if(frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher { //判断该点落在了那个cell内，则对应的cell内增加候选点； const int k = static_cast<int>(px[1]/grid_.cell_size)*grid_.grid_n_cols + static_cast<int>(px[0]/grid_.cell_size); grid_.cells.at(k)->push_back(Candidate(point, px)); return true; } return false; }
4.投影每个cell，比较重要，
bool Reprojector::reprojectCell(Cell& cell, FramePtr frame) { cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));//质量排序； Cell::iterator it=cell.begin(); //遍历cell中的Candidate； while(it!=cell.end()) { ++n_trials_; if(it->pt->type_ == Point::TYPE_DELETED) { it = cell.erase(it); continue; } //通过光流法估计patch的位置； bool found_match = true; if(options_.find_match_direct) found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px); if(!found_match) { it->pt->n_failed_reproj_++; if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15) map_.safeDeletePoint(it->pt); if(it->pt->type_ == Point::TYPE_CANDIDATE && it->pt->n_failed_reproj_ > 30) map_.point_candidates_.deleteCandidatePoint(it->pt); it = cell.erase(it); continue; } //投影成功+1； it->pt->n_succeeded_reproj_++; if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10) it->pt->type_ = Point::TYPE_GOOD; //给当前帧中增加新的feature Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_); frame->addFeature(new_feature); // Here we add a reference in the feature to the 3D point, the other way // round is only done if this frame is selected as keyframe. new_feature->point = it->pt; if(matcher_.ref_ftr_->type == Feature::EDGELET) { new_feature->type = Feature::EDGELET; new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad; new_feature->grad.normalize(); } // If the keyframe is selected and we reproject the rest, we don't have to // check this point anymore. it = cell.erase(it); //每个cell里面最多有一个point； // Maximum one point per cell. return true; } return false; }
5.光流法，逆向组合式：
patch_with_border_和patch_的区别是前者是带有border的patch，因为后面光流计算时需要计算梯度方向，使用的平均梯度，所以计算边界梯度时需要增加一行或一列；
bool Matcher::findMatchDirect( const Point& pt, const Frame& cur_frame, Vector2d& px_cur) { //计算cur_frame与观察到pt该点的特征中视角最小的一帧； if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_)) { return false; } //判断是否在图像内部； if(!ref_ftr_->frame->cam_->isInFrame( ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level)) { return false; } //计算仿射变换矩阵2*2； // warp affine 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); //计算该特征的仿射变换，由当前帧到参考帧； 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_); createPatchFromPatchWithBorder(); // px_cur should be set变换到搜索尺度下 Vector2d px_scaled(px_cur/(1<<search_level_)); //前面已经计算了参考帧到当前帧的仿射变换，并且已经获得了对应的patch； 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_); return success; }
6.计算获得与ftr视角最小的一帧，地图中关键帧不多，速度比较快：
bool Point::getCloseViewObs(const Vector3d& framepos, Feature*& ftr) const { // TODO: get frame with same point of view AND same pyramid level! Vector3d obs_dir(framepos - pos_); obs_dir.normalize(); auto min_it=obs_.begin();//观察到该点的关键帧列表； double min_cos_angle = 0; for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) { //计算该关键帧与该点的距离； Vector3d dir((*it)->frame->pos() - pos_); dir.normalize(); //计算余弦值； double cos_angle = obs_dir.dot(dir); //获取视角最小的关键帧； if(cos_angle > min_cos_angle) { min_cos_angle = cos_angle; min_it = it; } } ftr = *min_it; //不能大于60° if(min_cos_angle < 0.5) // assume that observations larger than 60° are useless return false; return true; }
7.计算仿射变换：即patch因为视角的变换，应该具有一定的扭曲
//计算仿射变换； void getWarpMatrixAffine( const svo::AbstractCamera& cam_ref, const svo::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) { // Compute affine warp matrix A_cur_ref const int halfpatch_size = 5;//5*5的窗口； const Vector3d xyz_ref(f_ref*depth_ref);//归一化的相机坐标乘以深度； //u方向的边界点和v方向的边界点，注意特征所在的金字塔level Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref))); // patch tranfrom to the level0 pyr img Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref))); // px_ref is located at level0 // attation!!!! so, A_cur_ref is only used to affine warp patch at level0 //因为xyz_du_ref返回的是归一化的3D坐标，所以要借助xyz_ref点的深度计算； xyz_du_ref *= xyz_ref[2]/xyz_du_ref[2]; xyz_dv_ref *= xyz_ref[2]/xyz_dv_ref[2]; //上面的三个点分别投影到当前帧； 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))); //仿射变换，其实是一种在x和y方向的变化率； A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size; A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size; }
8.通过仿射变换计算patch值：
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 ; 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; } // Perform the warp on a larger patch. uint8_t* patch_ptr = patch; 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) { Vector2f px_patch(x-halfpatch_size, y-halfpatch_size); // px_patch is locat at pyr [ref_level ] px_patch *= (1<<search_level);// 1. patch tranform to level0, because A_ref_cur is only used to affine warp level0 patch const Vector2f px(A_ref_cur*px_patch + px_ref_pyr); // 2. then, use A_ref_cur to affine warp the patch 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) interpolateMat_8u(img_ref, px[0], px[1]); // img_ref is the img at pyr[level] } } } }
9.给带边界的patch赋值：
//主要是给patch_填值； void Matcher::createPatchFromPatchWithBorder() { uint8_t* ref_patch_ptr = patch_; //为什么从1开始?因为横纵都+1，为什么横纵都+2，因为后面光流时要计算梯度，用到了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; for(int x=0; x<patch_size_; ++x) ref_patch_ptr[x] = ref_patch_border_ptr[x]; } }
10.光流法的主要过程：
bool align2D( const cv::Mat& cur_img, uint8_t* ref_patch_with_border, uint8_t* ref_patch, const int n_iter, Vector2d& cur_px_estimate, bool no_simd) { const int halfpatch_size_ = 4; const int patch_size_ = 8; const int patch_area_ = 64; bool converged=false; // compute derivative of template and prepare inverse compositional float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_]; float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_]; Matrix3f H; H.setZero(); // compute gradient and hessian const int ref_step = patch_size_+2; float* it_dx = ref_patch_dx; float* it_dy = ref_patch_dy; for(int y=0; y<patch_size_; ++y) { uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1; for(int x=0; x<patch_size_; ++x, ++it, ++it_dx, ++it_dy) { Vector3f J; //计算梯度方向； J[0] = 0.5 * (it[1] - it[-1]); J[1] = 0.5 * (it[ref_step] - it[-ref_step]); J[2] = 1; //梯度赋值，即保存每个像素点的梯度信息，因为是逆向组合算法，所以计算的是参考帧上的梯度信息； *it_dx = J[0]; *it_dy = J[1]; H += J*J.transpose(); } } Matrix3f Hinv = H.inverse(); float mean_diff = 0; //估算当前帧中的位置，因为前面的直接法已经有了光流初始值； // Compute pixel location in new image: float u = cur_px_estimate.x(); float v = cur_px_estimate.y(); // termination condition const float min_update_squared = 0.03*0.03; // origin param: 0.03 * 0.03 const int cur_step = cur_img.step.p[0]; // float chi2 = 0; //开始迭代优化； Vector3f update; update.setZero(); for(int iter = 0; iter<n_iter; ++iter) { int u_r = floor(u); int v_r = floor(v); //判断是否越界；应该是不会； if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_) break; if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check return false; //双线性插值权重 // compute interpolation weights float subpix_x = u-u_r; float subpix_y = v-v_r; float wTL = (1.0-subpix_x)*(1.0-subpix_y); float wTR = subpix_x * (1.0-subpix_y); float wBL = (1.0-subpix_x)*subpix_y; float wBR = subpix_x * subpix_y; // loop through search_patch, interpolate uint8_t* it_ref = ref_patch; float* it_ref_dx = ref_patch_dx; float* it_ref_dy = ref_patch_dy; // float new_chi2 = 0.0; Vector3f Jres; Jres.setZero(); for(int y=0; y<patch_size_; ++y) { uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_; for(int x=0; x<patch_size_; ++x, ++it, ++it_ref, ++it_ref_dx, ++it_ref_dy) { float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1]; //计算当前帧和参考帧patch像素点之间的残差； float res = search_pixel - *it_ref + mean_diff; //残差乘以梯度为b； Jres[0] -= res*(*it_ref_dx); Jres[1] -= res*(*it_ref_dy); Jres[2] -= res; // new_chi2 += res*res; } } //更新值为Hinv*b； update = Hinv * Jres; u += update[0]; v += update[1]; mean_diff += update[2]; if(update[0]*update[0]+update[1]*update[1] < min_update_squared) { converged=true; break; } } cur_px_estimate << u, v; return converged; }
通过以上函数，完成了基于patch的特征匹配，后续就是通过高斯牛顿法优化相机位姿了。
