public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

combined reprojection functions in new reprojector class

Browse Source
This commit is contained in:
Christian Forster
2014-03-31 19:31:11 +02:00
parent ac8b7be117
commit 8cf1504f7c
7 changed files with 160 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/src/frame_handler_base.cpp
-1
View File
@@ -25,7 +25,6 @@
#include <svo/matcher.h>
#include <svo/map.h>
#include <svo/point.h>
#include <svo/reprojection.h>
#include <svo/depth_filter.h>
namespace svo
svo/src/frame_handler_mono.cpp
+6 -8
View File
@@ -33,10 +33,9 @@ namespace svo {
FrameHandlerMono::
FrameHandlerMono(vk::AbstractCamera* cam) :
FrameHandlerBase(),
cam_(cam)
{
reprojection_grid_.initialize(cam_);
}
cam_(cam),
reprojector_(cam_, map_)
{}
FrameHandlerMono::
~FrameHandlerMono()
@@ -141,11 +140,10 @@ processFrame()
// map reprojection & feature alignment
SVO_START_TIMER("reproject");
size_t repr_n_new_references, repr_n_mps;
reprojection::reprojectMap(
map_, new_frame_, reprojection_grid_, map_.point_candidates_,
overlap_kfs_, repr_n_new_references, repr_n_mps);
reprojector_.reprojectMap(new_frame_, overlap_kfs_);
SVO_STOP_TIMER("reproject");
const size_t repr_n_new_references = reprojector_.n_matches_;
const size_t repr_n_mps = reprojector_.n_trials_;
SVO_LOG2(repr_n_mps, repr_n_new_references);
SVO_DEBUG_STREAM("Reprojection:\t nPoints = "<<repr_n_mps<<"\t \t nMatches = "<<repr_n_new_references);
if(repr_n_new_references == 0) {
svo/src/reprojection.cpp → svo/src/reprojector.cpp
+52 -63
View File
@@ -16,7 +16,7 @@
#include <algorithm>
#include <stdexcept>
#include <svo/reprojection.h>
#include <svo/reprojector.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/feature.h>
@@ -29,61 +29,59 @@
#include <vikit/timer.h>
namespace svo {
namespace reprojection {
Grid::Grid()
{}
void Grid::initialize(vk::AbstractCamera* cam)
Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map) :
map_(map)
{
cell_size = Config::gridSize();
grid_n_cols = ceil(static_cast<double>(cam->width())/cell_size);
grid_n_rows = ceil(static_cast<double>(cam->height())/cell_size);
cells.resize(grid_n_cols*grid_n_rows);
std::for_each(cells.begin(), cells.end(), [&](Cell*& c){ c = new Cell; });
cell_order.resize(cells.size());
for(size_t i=0; i<cells.size(); ++i)
cell_order[i] = i;
random_shuffle(cell_order.begin(), cell_order.end()); // maybe we should do it at every iteration!
initializeGrid(cam);
}
Grid::~Grid()
Reprojector::~Reprojector()
{
std::for_each(cells.begin(), cells.end(), [&](Cell* c){ delete c; });
cells.clear();
std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ delete c; });
}
void resetGrid(Grid& grid)
void Reprojector::initializeGrid(vk::AbstractCamera* cam)
{
std::for_each(grid.cells.begin(), grid.cells.end(), [&](Cell* c){ c->clear(); });
grid_.cell_size = Config::gridSize();
grid_.grid_n_cols = ceil(static_cast<double>(cam->width())/grid_.cell_size);
grid_.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()); // maybe we should do it at every iteration!
}
void reprojectMap(
Map& map,
void Reprojector::resetGrid()
{
n_matches_ = 0;
n_trials_ = 0;
std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ c->clear(); });
}
void Reprojector::reprojectMap(
FramePtr frame,
Grid& grid,
MapPointCandidates& candidate_points,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs,
size_t& n_matches,
size_t& n_trials)
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
{
resetGrid(grid);
resetGrid();
// 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);
map_.getCloseKeyframes(frame, close_kfs);
// Sort KFs with overlap according to their closeness // TODO is this still necessary?
// 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, max_n = 10;
overlap_kfs.reserve(max_n);
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<max_n; ++it_frame, ++n)
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));
@@ -100,7 +98,7 @@ void reprojectMap(
if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
continue;
(*it_ftr)->point->last_projected_kf_id_ = frame->id_;
if(reprojectPoint(grid, frame, (*it_ftr)->point))
if(reprojectPoint(frame, (*it_ftr)->point))
overlap_kfs.back().second++;
}
}
@@ -109,17 +107,17 @@ void reprojectMap(
// Now project all point candidates
SVO_START_TIMER("reproject_candidates");
{
boost::unique_lock<boost::mutex> lock(candidate_points.mut_);
auto it=candidate_points.candidates_.begin();
while(it!=candidate_points.candidates_.end())
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(grid, frame, it->first))
if(!reprojectPoint(frame, it->first))
{
it->first->n_failed_reproj_ += 3;
if(it->first->n_failed_reproj_ > 30)
{
candidate_points.deleteCandidate(*it);
it = candidate_points.candidates_.erase(it);
map_.point_candidates_.deleteCandidate(*it);
it = map_.point_candidates_.candidates_.erase(it);
continue;
}
}
@@ -131,38 +129,32 @@ void reprojectMap(
// 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");
n_matches=0; n_trials=0;
for(size_t i=0; i<grid.cells.size(); ++i)
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)
if(reprojectCell(*grid.cells.at(grid.cell_order[i]), frame, map, candidate_points, n_trials))
++n_matches;
if(n_matches > (size_t) Config::maxFts())
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)
bool Reprojector::pointQualityComparator(Candidate& lhs, Candidate& rhs)
{
if(lhs.pt->type_ > rhs.pt->type_)
return true;
return false;
}
bool reprojectCell(
Cell& cell,
FramePtr frame,
Map& map,
MapPointCandidates& candidate_points,
size_t& n_trials)
bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
{
cell.sort(boost::bind(&pointQualityComparator, _1, _2));
cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
Cell::iterator it=cell.begin();
while(it!=cell.end())
{
++n_trials;
++n_trials_;
if(it->pt->type_ == Point::DELETED)
{
@@ -176,9 +168,9 @@ bool reprojectCell(
{
it->pt->n_failed_reproj_++;
if(it->pt->type_ == Point::UNKNOWN_QUALITY && it->pt->n_failed_reproj_ > 15)
map.safeDeletePoint(it->pt);
map_.safeDeletePoint(it->pt);
if(it->pt->type_ == Point::CANDIDATE && it->pt->n_failed_reproj_ > 30)
candidate_points.deleteCandidatePoint(it->pt);
map_.point_candidates_.deleteCandidatePoint(it->pt);
it = cell.erase(it);
continue;
}
@@ -201,20 +193,17 @@ bool reprojectCell(
return false;
}
bool reprojectPoint(
Grid& grid,
FramePtr frame,
Point* point)
bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
{
Vector2d px(frame->w2c(point->pos_));
if(frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
{
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));
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;
}
} // namespace reprojection
} // namespace svo