这篇文章

这次仍是关于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的深度滤波等部分内容，也有会写一下自己对于逆向光流法的理解（冲冲冲!）。

SVO学习笔记（二）相关推荐

SVO 学习笔记(三)
SVO 学习笔记(三) 这篇博客 Initialization Frame_Handler_Mono processFirstFrame processSecondFrame processFrame ...
SVO 学习笔记（深度滤波）
SVO 学习笔记(深度滤波) 这篇博客论文中的深度滤波深度滤波的代码流程更新Seed对象初始化Seed对象结尾这篇博客这篇博客将介绍SVO论文中的Mapping部分,主要介绍深度滤波器 ...
SVO学习笔记（一）
SVO学习笔记(一) 这篇文章 Frame Featuredetection Featrue_matcher 三角测量求深度特征匹配非线性优化寻找匹配特征极线搜索匹配特征总结这篇文章一个 ...
qml学习笔记(二)：可视化元素基类Item详解（上半场anchors等等）
原博主博客地址:http://blog.csdn.net/qq21497936 本文章博客地址:http://blog.csdn.net/qq21497936/article/details/7851 ...
[转载]dorado学习笔记(二)
原文地址:dorado学习笔记(二)作者:傻掛 ·isFirst, isLast在什么情况下使用?在遍历dataset的时候会用到 ·dorado执行的顺序,首先由jsp发送请求,调用相关的ViewM ...
PyTorch学习笔记(二)——回归
PyTorch学习笔记(二)--回归本文主要是用PyTorch来实现一个简单的回归任务. 编辑器:spyder 1.引入相应的包及生成伪数据 import torch import torch.nn ...
tensorflow学习笔记二——建立一个简单的神经网络拟合二次函数
tensorflow学习笔记二--建立一个简单的神经网络 2016-09-23 16:04 2973人阅读评论(2) 收藏举报分类: tensorflow(4) 目录(?)[+] 本笔记目的 ...
Scapy学习笔记二
Scapy学习笔记二 Scapy Sniffer的用法: http://blog.csdn.net/qwertyupoiuytr/article/details/54670489 Scapy Snif ...
Ethernet/IP 学习笔记二
Ethernet/IP 学习笔记二原文链接:http://wiki.mbalib.com/wiki/Ethernet/IP 1.通信模式不同于源/目的通信模式,EtherNet/IP 采用生产/消 ...

SVO学习笔记（二）