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/CMakeLists.txt
+2 -2
View File
@@ -2,7 +2,7 @@
# user build settings
SET(DEBUG FALSE)
SET(TRACE FALSE)
SET(TRACE TRUE)
SET(HAVE_G2O FALSE)
################################################################################
@@ -109,7 +109,7 @@ LIST(APPEND SOURCEFILES
src/pose_optimizer.cpp
src/initialization.cpp
src/matcher.cpp
src/reprojection.cpp
src/reprojector.cpp
src/feature_alignment.cpp
src/feature_detection.cpp
src/depth_filter.cpp
svo/include/svo/frame_handler_mono.h
+2 -2
View File
@@ -19,7 +19,7 @@
#include <vikit/abstract_camera.h>
#include <svo/frame_handler_base.h>
#include <svo/reprojection.h>
#include <svo/reprojector.h>
#include <svo/initialization.h>
namespace svo {
@@ -49,7 +49,7 @@ public:
protected:
vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit).
reprojection::Grid reprojection_grid_; //!< Grid used for reprojection to make sure we have evenly distributed features.
Reprojector reprojector_; //!< Projects points from other keyframes into the current frame
FramePtr new_frame_; //!< Current frame.
FramePtr last_frame_; //!< Last frame, not necessarily a keyframe.
set<FramePtr> core_kfs_; //!< Keyframes in the closer neighbourhood.
svo/include/svo/reprojection.h
-98
View File
@@ -1,98 +0,0 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef SVO_REPROJECTION_H_
#define SVO_REPROJECTION_H_
#include <svo/global.h>
#include <svo/matcher.h>
namespace vk {
class AbstractCamera;
}
namespace svo {
class Map;
class Point;
class MapPointCandidates;
/// Project points from the map into the image and find the corresponding
/// feature (corner). We don't search a match for every point but only for one
/// point per cell. Thereby, we achieve a homogeneously distributed set of
/// matched features and at the same time we can save processing time by not
/// projecting all points.
namespace reprojection {
/// A candidate is a point that projects into the image plane and for which we
/// will search a maching feature in the image.
struct Candidate {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Point* pt; //!< 3D point.
Vector2d px; //!< projected 2D pixel location.
Candidate(Point* pt, Vector2d& px)
: pt(pt),
px(px)
{}
};
typedef std::list<Candidate, aligned_allocator<Candidate> > Cell;
typedef std::vector<Cell*> CandidateGrid;
/// 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;
Grid();
void initialize(vk::AbstractCamera* cam);
~Grid();
};
/// Reset the grid
void resetGrid(Grid& grid);
/// Project points from the map into the image. First finds keyframes with
/// overlapping field of view and projects only those map-points.
void reprojectMap(
Map& map,
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);
bool reprojectCell(
Cell& cell,
FramePtr frame,
Map& map,
MapPointCandidates& candidate_points,
size_t& n_trials);
bool reprojectPoint(
Grid& grid,
FramePtr frame,
Point* point);
} // namespace reprojection
} // namespace svo
#endif // SVO_REPROJECTION_H_
svo/include/svo/reprojector.h
+98
View File
@@ -0,0 +1,98 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef SVO_REPROJECTION_H_
#define SVO_REPROJECTION_H_
#include <svo/global.h>
#include <svo/matcher.h>
namespace vk {
class AbstractCamera;
}
namespace svo {
class Map;
class Point;
/// Project points from the map into the image and find the corresponding
/// feature (corner). We don't search a match for every point but only for one
/// point per cell. Thereby, we achieve a homogeneously distributed set of
/// matched features and at the same time we can save processing time by not
/// projecting all points.
class Reprojector
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Reprojector config parameters
struct Options {
size_t max_n_kfs; //!< max number of keyframes to reproject from
Options()
: max_n_kfs(10)
{}
} options_;
size_t n_matches_;
size_t n_trials_;
Reprojector(vk::AbstractCamera* cam, Map& map);
~Reprojector();
/// Project points from the map into the image. First finds keyframes with
/// overlapping field of view and projects only those map-points.
void reprojectMap(
FramePtr frame,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs);
private:
/// A candidate is a point that projects into the image plane and for which we
/// will search a maching feature in the image.
struct Candidate {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Point* pt; //!< 3D point.
Vector2d px; //!< projected 2D pixel location.
Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {}
};
typedef std::list<Candidate, aligned_allocator<Candidate> > Cell;
typedef std::vector<Cell*> CandidateGrid;
/// 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;
};
Grid grid_;
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);
};
} // namespace svo
#endif // SVO_REPROJECTION_H_
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