SVO-9-reprojector–2021.2.2
reprojector.h
#include <svo/global.h>
#include <svo/matcher.h>
namespace vk{
class AbstractCamera;
}
namespace svo{
class Map;
class Point;
class Reprojector{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
struct Options{
size_t max_n_kfs;
bool find_match_direct;
Options():max_n_kfs(10),find_match_direct(true){}
} options_;
size_t n_matches_;
size_t n_trials_;
Reprojector(vk::AbstractCamera* cam,Map& map);
~Reprojector();
void reprojectMap(FramePtr frame,
std::vector<std::pair<FramePtr,std::size_t>& overlap_kfs>;
private:
struct Candidate{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Point* pt;
Vector2d px;
Candidate(Point* pt,Vector2d& px): pt(pt),px(px){}
};
typedef std::list<Candidate> Cell;
typedef std::vector<Cell*> CandidateGrid;
struct Grid{
CandidateGrid cells;
vector<int> cell_order;
int cell_size;
int grid_n_cols;
int grid_n_rows;
};
Grid grid_;
Matcher matcher_;
Map& map_;
static bool pointQualityComparator(Candidate& lhs,Candidate& rhs);
void initializeGrid(vk::AbstractCamera* cam);
void resetGrid();
bool reprojectCell(Cell& cell,FramePtr frame);
bool reprojectPoint(FramePtr frame,Point* point);
};
}
#endif
reprojector.cpp
#include <algorithm>
#include <stdexcept>
#include <svo_practice/reprojector.h>
#include <svo_practice/frame.h>
#include <svo_practice/point.h>
#include <svo_practice/feature.h>
#include <svo_practice/map.h>
#include <svo_practice/config.h>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <vikit/abstract_camera.h>
#include <vikit/math_utils.h>
#include <vikit/timer.h>
namespace svo{
svo::Reprojector(vk::AbstractCamera* cam, svo::Map& map): map_(map)
{
initializeGrid(cam);
}
svo::~Reprojector()
{
std::for_each(grid_.cells.begin(),grid_.cells.end(),[&](Cell* c){delete c;});
}
void initializeGrid(vk::AbstractCamera* cam)
{
grid_.cell_size = Config::gridSize();
grid_.grid_n_cols = ceil(static_cast<double>(cam -> width())/grid_.cell_size);
gird_.grid_n_rows = ceil(static_cast<double>(cam -> height())/grid_.cell_size);
grid_.cells.resize(grid_.grid_n_cols * grid_.grid_n_rows);
std::for_each(grid_.cells.begin(),grid_.cells.end(),[&](Cell*& c){c = new Cell;});
grid_.cell_order.resize(grid_.cells.size());
for(size_t i = 0;i < grid_.cells.size();++i){
grid_.cell_order[i] = i;
}
random_shuffle(grid_.cell_order.begin(),grid_.cell_order.end());
}
void resetGrid()
{
n_matches_ = 0;
n_trials_ = 0;
std::for_each(grid_.cell.begin(),grid_.cell.end(),[&](Cell* c){c -> clear();})
}
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;
map_.getCloseKeyframes(frame,close_kfs);
close_kfs.sort(boost::bind(&std::pair<FramePtr,double>::second,_1) <
boost::bind(&std::pair<FramePtr,double>::second,_2));
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;
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){
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++;
}
}
}
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)){
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;
}
}
SVO_STOP_TIMER("reproject_candidates");
SVO_START_TIMER("feature_align");
for(size_t i = 0;i < grid_.cells.size();++i){
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");
}
}
bool pointQualityComparator(Candidate& lhs, Candidate& rhs)
{
if(lhs.pt -> type_ > rhs.pt -> type_)
return true;
return false;
}
bool reprojectCell(Cell& cell, FramePtr frame)
{
cell.sort(boost::bind(&Reprojector::pointQualityComparator,_1,_2));
Cell::iterator it = cell.begin();
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 -> it,*frame,it -> px);
}
if(!found_match){
it -> pt -> n_failed_reproj_++;
if(it -> pt -> type_ == Point::TYPE_UNKONWN && 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_successed_reproj_++;
if(it -> pt -> type_ == Point::TYPE_UNKONWN && it -> pt -> n_successed_reproj_ > 10){
it -> pt -> type_ = Point::TYPE_GOOD;
}
Feature* new_feature = new Feature(frame.get(),it -> px,matcher_.search_level_);
frame -> addFrame(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;
}
bool reprojectPoint(FramePtr frame, Point* point)
{
Vector2d px(frame -> w2c(point -> pos_));
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;
}