PL-SVO公式推导及代码解析:地图点重投影和特征对齐
对当前帧进行地图点重投影和特征对齐
// map reprojection & feature alignment SVO_START_TIMER("reproject"); reprojector_.reprojectMap(new_frame_, overlap_kfs_); SVO_STOP_TIMER("reproject");
在processframe函数中在进行初始的稀疏图像对齐之后，进一步进行地图投影和特征对齐，对新一帧图像添加特征点，由reprojectMap接口进入
Reprojector reprojector_; //把其它关键帧中的点投影到当前帧中
FramePtr new_frame_; // 当前帧
vector< pair<FramePtr,size_t> > overlap_kfs_; // 具有重叠视野的所有关键帧。 配对数字指明观察到的公共地图点数。
void Reprojector::reprojectMap( FramePtr frame, std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs) { resetReprojGrid();
resetReprojGrid(); // 重置函数
// Identify those Keyframes which share a common field of view. SVO_START_TIMER("reproject_kfs"); list< pair<FramePtr,double> > close_kfs; map_.getCloseKeyframes(frame, close_kfs);
void Map::getCloseKeyframes(
const FramePtr& frame,
std::list< std::pair<FramePtr,double> >& close_kfs);
const FramePtr& frame; //当前图像帧
std::list< std::pair<FramePtr,double> >& close_kfs ；///存储和当前帧接近的关键帧指针以及它们之间的距离。
根据它们的接近程度对KF进行重叠排序（列表中的第二个值）
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) < boost::bind(&std::pair<FramePtr, double>::second, _2));
重新投影具有重叠的最近N kfs的所有地图特征。
size_t n_kfs = 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_kfs<options_.max_n_kfs; ++it_frame, ++n_kfs) { FramePtr ref_frame = it_frame->first; // add the current frame to the (output) list of keyframes with overlap // initialize the counter of candidates from this frame (2nd element in pair) to zero overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0)); // Consider for candidate each mappoint in the ref KF that the current (input) KF observes // We only store in which grid cell the points fall. // Add each corresponding valid new Candidate to its cell in the grid. int num_pt_success = setKfCandidates( frame, ref_frame->pt_fts_ ); overlap_kfs.back().second += num_pt_success; // Add each line segment in the ref KF that the current (input) KF observes int num_seg_success = setKfCandidates( frame, ref_frame->seg_fts_ ); overlap_kfs.back().second += num_seg_success; } SVO_STOP_TIMER("reproject_kfs");
考虑当前（输入）KF观察到的参考KF中的每个mappoint的候选者
我们只存储点落在哪个网格单元格中。
将每个对应的有效新Candidate添加到网格中的单元格中。
template<class FeatureT> int Reprojector::setKfCandidates(FramePtr frame, list<FeatureT*> fts) { int candidate_counter = 0; for(auto it=fts.begin(), ite_ftr=fts.end(); it!=ite_ftr; ++it) { // check if the feature has a 3D object assigned if((*it)->feat3D == NULL) continue; // make sure we project a point only once if((*it)->feat3D->last_projected_kf_id_ == frame->id_) continue; (*it)->feat3D->last_projected_kf_id_ = frame->id_; if(reproject(frame, (*it)->feat3D)) // increment the number of candidates taken successfully candidate_counter++; } return candidate_counter; }
int Reprojector::setKfCandidates(FramePtr frame, list<FeatureT*> fts)
把地图点和投影到当前帧中的像素点的匹配对存储到 grid_.cells 中
bool Reprojector::reproject(FramePtr frame, Point* point) { // get position in current frame image of the world 3D point Vector2d cur_px(frame->w2c(point->pos_)); if(frame->cam_->isInFrame(cur_px.cast<int>(), 8)) // 8px is the patch size in the matcher { // get linear index (wrt row-wise vectorized grid matrix) // of the image grid cell in which the point px lies const int k = static_cast<int>(cur_px[1]/grid_.cell_size)*grid_.grid_n_cols + static_cast<int>(cur_px[0]/grid_.cell_size); grid_.cells.at(k)->push_back(PointCandidate(point, cur_px)); return true; } return false; }
数据类型的定义
struct PointCandidate { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Point* pt; //!< 3D point. Vector2d px; //!< projected 2D pixel location. PointCandidate(Point* pt, Vector2d& px) : pt(pt), px(px) {} };
// A cell is just a list of Reprojector::Candidate typedef std::list<PointCandidate, aligned_allocator<PointCandidate> > Cell; typedef std::vector<Cell*> CandidateGrid; struct LineCandidate { EIGEN_MAKE_ALIGNED_OPERATOR_NEW LineSeg* ls; //!< 3D point. Vector2d spx; Vector2d epx; LineCandidate(LineSeg* ls, Vector2d& spx, Vector2d& epx) : ls(ls), spx(spx), epx(epx) {} };
/// The candidate segments are collected in a single list for the whole image. /// There is no clear heuristic about how to discretize the image space for the segment case. typedef std::list<LineCandidate, aligned_allocator<LineCandidate> > LineCandidates; typedef std::vector<LineCandidates*> LineCandidateGrid; /// The grid stores a set of candidate matches. For every grid cell we try to find one match. struct Grid { CandidateGrid cells; vector<int> cell_order; int cell_size; int grid_n_cols; int grid_n_rows; }; struct GridLs { LineCandidateGrid cells; vector<int> cell_order; int cell_size; int grid_n_cols; int grid_n_rows; }; Grid grid_; GridLs gridls_; Matcher matcher_; Map& map_;
现在投影所有地图坐标
点坐标
// Now project all map candidates // (candidates in the map are created from converged seeds) SVO_START_TIMER("reproject_candidates"); // Point candidates // (same logic as above to populate the cell grid but taking candidate points from the map object) setMapCandidates(frame, map_.point_candidates_);
线坐标
// Segment candidates setMapCandidates(frame, map_.segment_candidates_); SVO_STOP_TIMER("reproject_candidates");
Map& map_; //地图类
MapPointCandidates point_candidates_; // 收敛的3D点的容器，尚未分配给两个关键帧。
MapSegmentCandidates segment_candidates_; /////尚未分配给两个关键帧的融合3D点的容器。
class MapPointCandidates { public: typedef pair<Point*, PointFeat*> PointCandidate; typedef list<PointCandidate> PointCandidateList; /// The depth-filter is running in a parallel thread and fills the canidate list. /// This mutex controls concurrent access to point_candidates. boost::mutex mut_; /// Candidate points are created from converged seeds. /// Until the next keyframe, these points can be used for reprojection and pose optimization. PointCandidateList candidates_; list< Point* > trash_points_; MapPointCandidates(); ~MapPointCandidates(); /// Add a candidate point. void newCandidatePoint(Point* point, double depth_sigma2); /// Adds the feature to the frame and deletes candidate from list. void addCandidatePointToFrame(FramePtr frame); /// Remove a candidate point from the list of candidates. bool deleteCandidatePoint(Point* point); /// Remove all candidates that belong to a frame. void removeFrameCandidates(FramePtr frame); /// Reset the candidate list, remove and delete all points. void reset(); void deleteCandidate(PointCandidate& c); void emptyTrash(); };
void MapPointCandidates::deleteCandidate(PointCandidate& c) { // camera-rig: another frame might still be pointing to the candidate point // therefore, we can't delete it right now. delete c.second; c.second=NULL; c.first->type_ = Point::TYPE_DELETED; trash_points_.push_back(c.first); }
template<class MapCandidatesT> void Reprojector::setMapCandidates(FramePtr frame, MapCandidatesT &map_candidates) { boost::unique_lock<boost::mutex> lock(map_candidates.mut_); // the mutex will be unlocked when out of scope auto it=map_candidates.candidates_.begin(); while(it!=map_candidates.candidates_.end()) { if(!reproject(frame, it->first)) { // if the reprojection of the map candidate point failed, // increment the counter of failed reprojections (assess the point quality) it->first->n_failed_reproj_ += 3; if(it->first->n_failed_reproj_ > 30) { // if the reprojection failed too many times, remove the map candidate point map_candidates.deleteCandidate(*it); it = map_candidates.candidates_.erase(it); continue; } } ++it; } // end-while-loop }
现在我们浏览每个网格单元并选择一个匹配点。最后，我们每个细胞最多应该有一个重新投射点。
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) // we use the random cell order to visit cells uniformly on the grid if(refineBestCandidate(*grid_.cells.at(grid_.cell_order[i]), frame)) ++n_matches_; if(n_matches_ > (size_t) Config::maxFts()) break; // the random visit order of cells assures uniform distribution // of the features even if we break early (maxFts reached soon) }
我们更喜欢质量好的点而不是未知质量（更可能匹配）和未知质量而不是候选者（位置未优化）
我们使用随机单元格顺序在网格上统一访问单元格
bool Reprojector::refineBestCandidate(Cell& cell, FramePtr frame) { // sort the candidates inside the cell according to their quality cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2)); Cell::iterator it=cell.begin(); // in principle, iterate through the whole list of features in the cell // in reality, there is maximum one point per cell, so the loop returns if successful while(it!=cell.end()) { ++n_trials_; // Try to refine the point feature in frame from current initial estimate bool success = refine( it->pt, it->px, frame ); // Failed or not, this candidate was finally being erased in original code it = cell.erase(it); // it takes next position in the list as output of .erase if(success) // Maximum one point per cell. return true; } return false; }
尝试从当前初始估计中细化帧中的点要素
bool Reprojector::refine(Point* pt, Vector2d& px_est, FramePtr frame) { if(pt->type_ == Point::TYPE_DELETED) return false; bool found_match = true; if(options_.find_match_direct) // refine px position in the candidate by directly applying subpix refinement // internally, it is optimizing photometric error // of the candidate px patch wrt the closest-view reference feature patch found_match = matcher_.findMatchDirect(*pt, *frame, px_est); // TODO: What happens if options_.find_match_direct is false??? Shouldn't findEpipolarMatchDirect be here? // apply quality logic { if(!found_match) { // if there is no match found for this point, decrease quality pt->n_failed_reproj_++; // consider removing the point from map depending on point type and quality if(pt->type_ == Point::TYPE_UNKNOWN && pt->n_failed_reproj_ > 15) map_.safeDeletePoint(pt); if(pt->type_ == Point::TYPE_CANDIDATE && pt->n_failed_reproj_ > 30) map_.point_candidates_.deleteCandidatePoint(pt); return false; } // if there is successful match found for this point, increase quality pt->n_succeeded_reproj_++; if(pt->type_ == Point::TYPE_UNKNOWN && pt->n_succeeded_reproj_ > 10) pt->type_ = Point::TYPE_GOOD; } // create new point feature for this frame with the refined (aligned) candidate position in this image PointFeat* new_feature = new PointFeat(frame.get(), px_est, 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. // TODO: why not give it directly to the constructor PointFeat(frame.get(), pt, it->px, matcher_.serach_level_) new_feature->feat3D = pt; PointFeat* pt_ftr = static_cast<PointFeat*>( matcher_.ref_ftr_ ); if(pt_ftr != NULL) { if(pt_ftr->type == PointFeat::EDGELET) { new_feature->type = PointFeat::EDGELET; new_feature->grad = matcher_.A_cur_ref_*pt_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); // Maximum one point per cell. return true; }
通过直接应用子像素细化来细化候选者中的px位置
在内部，它正在优化最近视图参考特征补丁的候选px补丁的光度误差
bool Matcher::findMatchDirect( const Point& pt, const Frame& cur_frame, Vector2d& px_cur) { // get reference feature in closest view (frame) 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; // 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); // is img of ref_frame fully available at any time? that means keeping stored previous images, for how long? 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_ stores the square patch (of pixel intensities) around the reference feature // once the affine transformation is applied to the original reference image // the border is necessary for gradient operations (intensities at the border must be precomputed by interpolation too!) createPatchFromPatchWithBorder(); // px_cur should be set Vector2d px_scaled(px_cur/(1<<search_level_)); bool success = false; PointFeat* pt_ftr = static_cast<PointFeat*>(ref_ftr_); if(pt_ftr->type == PointFeat::EDGELET) { Vector2d dir_cur(A_cur_ref_*pt_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; }
特征细化
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) { #ifdef __ARM_NEON__ if(!no_simd) return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate); #endif const int patch_size_ = 8; Patch patch( patch_size_, cur_img ); bool converged=false; /* Precomputation step: derivatives in reference patch */ // 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; // assumes ref_patch_with_border comes from a specific Mat object with certain size!!! Bad way to do it? 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; /* Iterative loop: residues and updates with patch in current image */ // 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; const int cur_step = cur_img.step.p[0]; // float chi2 = 0; Vector3f update; update.setZero(); for(int iter = 0; iter<n_iter; ++iter) { // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check // if(isnan(cur_px_estimate[0]) || isnan(cur_px_estimate[1])) // return false; // set patch position for current feature location patch.setPosition( cur_px_estimate ); // abort the optimization if the patch does not fully lie within the image if(!patch.isInFrame(patch.halfsize)) break; // compute interpolation weights patch.computeInterpWeights(); // set ROI in the current image to traverse patch.setRoi(); // 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; uint8_t* ptr; // pointer that will point to memory locations of the ROI (same memory as for the original full cur_img) // float new_chi2 = 0.0; Vector3f Jres; Jres.setZero(); for(int y=0; y<patch.size; ++y) { // get the pointer to first element in row y of the patch ROI ptr = patch.roi.ptr(y); for(int x=0; x<patch.size; ++x, ++ptr, ++it_ref, ++it_ref_dx, ++it_ref_dy) { float search_pixel = patch.wTL*ptr[0] + patch.wTR*ptr[1] + patch.wBL*ptr[cur_step] + patch.wBR*ptr[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; // new_chi2 += res*res; } } /* if(iter > 0 && new_chi2 > chi2) { #if SUBPIX_VERBOSE cout << "error increased." << endl; #endif u -= update[0]; v -= update[1]; break; } chi2 = new_chi2; */ update = Hinv * Jres; u += update[0]; v += update[1]; cur_px_estimate = Vector2d(u,v); mean_diff += update[2]; #if SUBPIX_VERBOSE cout << "Iter " << iter << ":" << "\t u=" << u << ", v=" << v << "\t update = " << update[0] << ", " << update[1] // << "\t new chi2 = " << new_chi2 << endl; #endif if(update[0]*update[0]+update[1]*update[1] < min_update_squared) { #if SUBPIX_VERBOSE cout << "converged." << endl; #endif converged=true; break; } } cur_px_estimate << u, v; return converged; }
//为当前创建新的点特征，并在此图像中使用精确（对齐）的位置坐标
PointFeat* new_feature = new PointFeat(frame.get(), px_est, matcher_.search_level_); frame->addFeature(new_feature);
在这里，我们将特征中的引用添加到3D点，只有在选择此帧作为关键帧时才进行另一种方式。
if(pt_ftr != NULL) { if(pt_ftr->type == PointFeat::EDGELET) { new_feature->type = PointFeat::EDGELET; new_feature->grad = matcher_.A_cur_ref_*pt_ftr->grad; new_feature->grad.normalize(); } }
尝试优化每个细分线段坐标
即使我们提前破坏，细胞的随机访问顺序也能确保特征的均匀分布（maxFts即将到达）
for(size_t i=0; i<gridls_.cells.size(); ++i) { if(refineBestCandidate(*gridls_.cells.at(gridls_.cell_order[i]), frame)) ++n_ls_matches_; if(n_ls_matches_ > (size_t) Config::maxFtsSegs()) break; // the random visit order of cells assures uniform distribution // of the features even if we break early (maxFts reached soon) } /*for(auto it = lines_.begin(), ite = lines_.end(); it!=ite; ++it) { if(refine(it->ls,it->spx,it->epx,frame)) ++n_ls_matches_; if(n_ls_matches_ > (size_t) Config::maxFtsSegs()) break; }*/ SVO_STOP_TIMER("feature_align");
特征细化结束
