这篇文章

这次仍是关于SVO系统的学习记录（冲冲冲！）。这次的主要内容是sparseimgalign、reproject和featurealign这三个代码文件。首先介绍第一个代码文件----稀疏图像对齐。

稀疏图像对齐

这部分代码是论文中“Sparse Model-based Image Alignment”部分的实现。它通过直接法在前后两帧中寻找匹配点，然后估计出两帧间的相对位姿变换矩阵（粗略估计）。这部分大家可以参考的这个博客。这篇博客对这部分代码讲解的十分详细且容易理解（向前辈学习！）。此处只记录一下run()的部分。

size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
{reset();if(ref_frame->fts_.empty()){SVO_WARN_STREAM("SparseImgAlign: no features to track!");return 0;}ref_frame_ = ref_frame;cur_frame_ = cur_frame;//参考帧中所有特征点的像素块（refpatches）。每一行对应一个特征点像素块内所有的像素ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);//jacobian_cache_保存各像素的雅可比矩阵//使用resize能一次性给容器分配好空间，之后pushback就只是放入元素。这样能节约一定时间jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_);//记录参考帧中哪些特征点被当前帧观测到了visible_fts_.resize(ref_patch_cache_.rows, false); SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse());//先使用高层的金字塔图像进行位姿优化（粗略优化），然后逐步使用低层金字塔图像进行位姿优化//（在高层找到匹配点后，再去低层优化匹配点位置）for(level_=max_level_; level_>=min_level_; --level_){mu_ = 0.1;jacobian_cache_.setZero();have_ref_patch_cache_ = false;if(verbose_)printf("\nPYRAMID LEVEL %i\n---------------\n", level_);optimize(T_cur_from_ref);}cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;return n_meas_/patch_area_;
}

地图点投影(地图与当前帧间的关系)

这个文件是将地图中的地图点投影到当前帧中，然后建立这些地图点的最初观测帧（关键帧）与当前帧间的联系，以便实现论文中的特征点对齐部分的内容。主要介绍文件中较为重要的几个函数，首先是reprojectMap函数。该函数将地图中的点投影到当前帧中，帮助建立关键帧和当前帧间的联系。

reprojectMap

//将地图点投影到当前帧中，建立关键帧与当前帧的联系
void Reprojector::reprojectMap(FramePtr frame,//当前帧//容器保存的是关键帧的指针和其与当前帧的共视程度std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)//关联上的关键帧
{resetGrid();SVO_START_TIMER("reproject_kfs");list< pair<FramePtr,double> > close_kfs;//获得与当前frame共视的KFsmap_.getCloseKeyframes(frame, close_kfs);//那五个特殊点派上了用场//根据KF与frame的距离对共视的KFs进行排序close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <boost::bind(&std::pair<FramePtr, double>::second, _2));size_t n = 0;//只取最前面几个KFsoverlap_kfs.reserve(options_.max_n_kfs);//统计与当前帧有共视的KFs，以及每个KF与当前frame有多少共视的地图点for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();it_frame!=ite_frame && n<options_.max_n_kfs; ++it_frame, ++n)//遍历距离近的所有KFs{FramePtr ref_frame = it_frame->first;//pair（与当前frame有共视部分的KF，共视的特征点的数量）overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end();it_ftr!=ite_ftr; ++it_ftr)//遍历KF中所有特征点（主要是看特征点对应的地图点是否在frame上可见）{if((*it_ftr)->point == NULL)continue;//防止对同一个地图点进行重复操作if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)continue;(*it_ftr)->point->last_projected_kf_id_ = frame->id_;if(reprojectPoint(frame, (*it_ftr)->point))//投影判断地图点是否在能在当前帧中overlap_kfs.back().second++;//共视点加1}}SVO_STOP_TIMER("reproject_kfs");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))//看候选地图点是否在frame中{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 scopeSVO_STOP_TIMER("reproject_candidates");SVO_START_TIMER("feature_align");//grid_：在reprojectPoint函数中配置好的，记录某地图点在当前图像中哪一个网格内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//使每个当前帧中的网格内只保留一个3D-2D匹配点对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");
}

上面的代码将地图点投影到当前帧中，为地图点在当前帧中找到了更好的匹配点，并建立相应关键帧与当前帧之间的关系。代码中要注意的两个函数是：1）reprojectPoint：判断地图点是否在当前帧中，并记录地图点投影在图像的哪个网格内；2）reprojectCell：保证每个网格内只保留一个地图点以及它的匹配点。这写操作能控制匹配点对的分布，同时减少计算量。下面来介绍这两个函数。

reprojectPoint

//判断某一个地图点是否在当前帧中
//如果在，则将该地图点分配到对应的网格中，并保存3D-2D匹配点对。
//此时网格中的所有3D-2D点对都只是最粗略的匹配，需要进一步优化匹配结果
bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
{Vector2d px(frame->w2c(point->pos_));// 保证特征点以及它周围的像素块（搜索匹配时要用）也能够在frame的图像范围内if(frame->cam_->isInFrame(px.cast<int>(), 8)) {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;
}

reprojectCell

/*函数所需参数：cell--网格对象frame--当前帧
功能：优化网格中的地图点与像素点（3D-2D点对）的匹配关系，并保证每个网格内只有一对匹配点对。
*/
bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
{//按照点的好坏程度进行排序cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));Cell::iterator it=cell.begin();//遍历cell中的Candidates类对象（3D-2D点对）while(it!=cell.end()){++n_trials_;if(it->pt->type_ == Point::TYPE_DELETED)//剔除坏点的匹配{it = cell.erase(it);continue;}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;}//更新地图点的状态。可以提高地图中的好点数it->pt->n_succeeded_reproj_++;if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)it->pt->type_ = Point::TYPE_GOOD;//给frame增加新的特征点Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);frame->addFeature(new_feature);//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();}it = cell.erase(it);//网格中找出一对精确的匹配点对，就不再关心网格中的剩余部分点对return true;}return false;
}

特征点对齐中的非线性优化

这部分被featurematcher文件所调用。这里介绍align2D函数中迭代求解的步骤，它是针对点特征的求解方法。函数中迭代求解的最小二乘问题是根据Inverse Compositional Image Alignment算法（我愿称它为逆向光流法）构建的。对这个算法的介绍可以参考这个博客。

//点特征的非线性优化搜索匹配
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)
{
//使用的是8X8的patchconst int halfpatch_size_ = 4;const int patch_size_ = 8;const int patch_area_ = 64;bool converged=false;//__attribute是C语言中的字节对齐操作，用来提高访问数据的效率float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_];//保存patch中每个点在x、y方向上的梯度float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_];Matrix3f H; H.setZero();//加2是因为这里处理的patch是带边框的const int ref_step = patch_size_+2;float* it_dx = ref_patch_dx;float* it_dy = ref_patch_dy;//论文中计算的是特征点那个框框中的光度误差，所以要对patch中每个点都进行误差计算//这里所求解的最小二乘问题由反向LK光流法构建。for(int y=0; y<patch_size_; ++y){//获得带边界Patch中的每行头指针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;//用差分来计算x、y方向上的光度值的导数（离散）J[0] = 0.5 * (it[1] - it[-1]);//dxJ[1] = 0.5 * (it[ref_step] - it[-ref_step]);//dyJ[2] = 1;*it_dx = J[0];*it_dy = J[1];//组成Hession矩阵H += J*J.transpose();}}Matrix3f Hinv = H.inverse();float mean_diff = 0;float u = cur_px_estimate.x();float v = cur_px_estimate.y();const float min_update_squared = 0.03*0.03;const int cur_step = cur_img.step.p[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.. checkreturn false;//亚像素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;uint8_t* it_ref = ref_patch;float* it_ref_dx = ref_patch_dx;float* it_ref_dy = ref_patch_dy;Vector3f Jres; Jres.setZero();for(int y=0; y<patch_size_; ++y){//获得匹配特征点在cur的patch中各元素的光度值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];float res = search_pixel - *it_ref + mean_diff;//雅可比矩阵乘以误差Jres[0] -= res*(*it_ref_dx);Jres[1] -= res*(*it_ref_dy);Jres[2] -= res;}}//计算坐标的更新量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;
}
//省略了代码中的一些输出项

结尾

以上就是整篇博客的全部内容。这篇博客介绍了SVO论文中的“Sparse Model-based Image Alignment”部分、“Relaxation Through Feature Alignment”部分中的重投影操作和迭代优化求解的实现。之后计划写有关SVO的深度滤波等部分内容，也有会写一下自己对于逆向光流法的理解（冲冲冲!）。