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

Merge branch 'dev' of https://github.com/uzh-rpg/rpg_svo into dev

Browse Source 
Conflicts:
	svo/src/reprojector.cpp
This commit is contained in:
Christian Forster
2014-04-03 10:20:20 +02:00
parent a1e014c77c 1f44610858
commit 0454a92de3
33 changed files with 607 additions and 632 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/include/svo/frame_handler_base.h
+26 -16
View File
@@ -26,13 +26,14 @@
#include <svo/global.h>
#include <svo/map.h>
namespace vk {
class AbstractCamera;
class PerformanceMonitor;
namespace vk
{
class AbstractCamera;
class PerformanceMonitor;
}
namespace svo {
namespace svo
{
class Point;
class Matcher;
class DepthFilter;
@@ -41,9 +42,22 @@ class DepthFilter;
class FrameHandlerBase : boost::noncopyable
{
public:
enum Stage { PAUSED, FIRST_FRAME, SECOND_FRAME, DEFAULT_FRAME };
enum TrackingQuality { INSUFFICIENT, BAD, GOOD };
enum UpdateResult { NO_KEYFRAME, IS_KEYFRAME, FAILURE };
enum Stage {
STAGE_PAUSED,
STAGE_FIRST_FRAME,
STAGE_SECOND_FRAME,
STAGE_DEFAULT_FRAME
};
enum TrackingQuality {
TRACKING_INSUFFICIENT,
TRACKING_BAD,
TRACKING_GOOD
};
enum UpdateResult {
RESULT_NO_KEYFRAME,
RESULT_IS_KEYFRAME,
RESULT_FAILURE
};
FrameHandlerBase();
@@ -73,9 +87,6 @@ public:
/// Get the number of feature observations of the last frame.
size_t lastNumObservations() const { return num_obs_last_; }
/// Access the depth filter.
DepthFilter* depthFilter() const { return depth_filter_; }
protected:
Stage stage_; //!< Current stage of the algorithm.
bool set_reset_; //!< Flag that the user can set. Will reset the system before the next iteration.
@@ -87,16 +98,15 @@ protected:
vk::RingBuffer<size_t> acc_num_obs_; //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance.
size_t num_obs_last_; //!< Number of observations in the previous frame.
TrackingQuality tracking_quality_; //!< An estimate of the tracking quality based on the number of tracked features.
DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
/// Before a frame is processed, this function is called.
bool startFrameProcessingCommon();
bool startFrameProcessingCommon(const double timestamp);
/// When a frame is finished processing, this function is called.
int finishFrameProcessingCommon(
size_t update_id,
UpdateResult dropout,
size_t num_observations);
const size_t update_id,
const UpdateResult dropout,
const size_t num_observations);
/// Reset the map and frame handler to start from scratch.
void resetCommon();
svo/include/svo/frame_handler_mono.h
+6 -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 {
@@ -47,14 +47,18 @@ public:
const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; }
const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; }
/// Access the depth filter.
DepthFilter* depthFilter() const { return depth_filter_; }
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.
vector< pair<FramePtr,size_t> > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!?
initialization::KltHomographyInit klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography.
DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
/// Processes the first frame and sets it as a keyframe.
UpdateResult processFirstFrame();
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
+100
View File
@@ -0,0 +1,100 @@
// 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
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();
/// 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/include/svo/sparse_img_align.h
+1 -1
View File
@@ -60,7 +60,7 @@ protected:
int min_level_; //!< finest pyramid level for the alignment.
// cache:
Matrix<double, 6, Dynamic, ColMajor> jacobian_cache_; //!< the first row is a flag whether the jacobian has been computed.
Matrix<double, 6, Dynamic, ColMajor> jacobian_cache_;
bool have_ref_patch_cache_;
cv::Mat ref_patch_cache_;
std::vector<bool> visible_fts_;