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
README.md
+4 -4
View File
@@ -91,7 +91,7 @@ Now we are ready to build SVO.
Clone it into your catkin workspace
cd catkin_ws/src
git clone https://github.com/uzh-rpg/rpg_slam.git
git clone https://github.com/uzh-rpg/rpg_svo.git
If you installed g2o then set `HAVE_G2O` in `svo/CMakeLists.txt` to TRUE.
Then build
@@ -165,9 +165,9 @@ Citing
If you use SVO in an academic context, please cite the following publication:
@article{Forster2014ICRA,
@inproceedings{Forster2014ICRA,
author = {Forster, Christian and Pizzoli, Matia and Scaramuzza, Davide},
title = {{SVO: Fast Semi-Direct Monocular Visual Odometry}},
journal = {IEEE International Conference on Robotics and Automation (ICRA)},
title = {{SVO}: Fast Semi-Direct Monocular Visual Odometry},
booktitle = {IEEE International Conference on Robotics and Automation (ICRA)},
year = {2014}
}
svo/CMakeLists.txt
+3 -10
View File
@@ -49,7 +49,7 @@ ELSE()
ENDIF()
# Add catkin and required ROS packages
FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp vikit_common vikit_ros)
FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib vikit_common vikit_ros)
# Add plain cmake packages
FIND_PACKAGE(G2O REQUIRED)
@@ -62,7 +62,7 @@ FIND_PACKAGE(Boost REQUIRED COMPONENTS system thread signals)
# Describe catkin Project
catkin_package(
DEPENDS Eigen OpenCV Sophus Boost fast
CATKIN_DEPENDS roscpp vikit_common vikit_ros
CATKIN_DEPENDS roscpp roslib vikit_common vikit_ros
INCLUDE_DIRS include
LIBRARIES svo
)
@@ -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
@@ -127,13 +127,6 @@ TARGET_LINK_LIBRARIES(svo ${LINK_LIBS})
################################################################################
# TESTS
IF(DEFINED ENV{SVO_DATASET_DIR})
ADD_DEFINITIONS(-DTEST_DATA_DIR=\"$ENV{SVO_DATASET_DIR}\")
ELSE()
ADD_DEFINITIONS(-DTEST_DATA_DIR=\"${PROJECT_SOURCE_DIR}/test/data\")
ENDIF()
ADD_DEFINITIONS(-DTEST_TRACE_DIR=\"${PROJECT_SOURCE_DIR}/test/results\")
ADD_EXECUTABLE(test_feature_align test/test_feature_alignment.cpp)
TARGET_LINK_LIBRARIES(test_feature_align svo)
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_;
svo/package.xml
+2
View File
@@ -14,11 +14,13 @@
<!-- Dependencies needed to compile this package. -->
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<build_depend>vikit_common</build_depend>
<build_depend>vikit_ros</build_depend>
<!-- Dependencies needed after this package is compiled. -->
<run_depend>roscpp</run_depend>
<run_depend>roslib</run_depend>
<run_depend>vikit_common</run_depend>
<run_depend>vikit_ros</run_depend>
</package>
svo/src/frame_handler_base.cpp
+33 -40
View File
@@ -25,26 +25,23 @@
#include <svo/matcher.h>
#include <svo/map.h>
#include <svo/point.h>
#include <svo/reprojection.h>
#include <svo/depth_filter.h>
namespace svo {
namespace svo
{
// definition of global and static variables which were declared in the header
#ifdef SVO_TRACE
vk::PerformanceMonitor* g_permon = NULL;
#endif
FrameHandlerBase::
FrameHandlerBase() :
stage_(PAUSED),
FrameHandlerBase::FrameHandlerBase() :
stage_(STAGE_PAUSED),
set_reset_(false),
set_start_(false),
acc_frame_timings_(10),
acc_num_obs_(10),
num_obs_last_(0),
tracking_quality_(BAD),
depth_filter_(NULL)
tracking_quality_(TRACKING_BAD)
{
#ifdef SVO_TRACE
// Initialize Performance Monitor
@@ -52,10 +49,14 @@ FrameHandlerBase() :
g_permon->addTimer("pyramid_creation");
g_permon->addTimer("sparse_img_align");
g_permon->addTimer("reproject");
g_permon->addTimer("reproject_kfs");
g_permon->addTimer("reproject_candidates");
g_permon->addTimer("feature_align");
g_permon->addTimer("pose_optimizer");
g_permon->addTimer("point_optimizer");
g_permon->addTimer("local_ba");
g_permon->addTimer("tot_time");
g_permon->addLog("timestamp");
g_permon->addLog("img_align_n_tracked");
g_permon->addLog("repr_n_mps");
g_permon->addLog("repr_n_new_references");
@@ -72,35 +73,26 @@ FrameHandlerBase() :
g_permon->init(Config::traceName(), Config::traceDir());
#endif
// create depth filter and set callback
feature_detection::DetectorPtr feature_detector(new feature_detection::FastDetector());
DepthFilter::callback_t depth_filter_cb = boost::bind(&MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2);
depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb);
depth_filter_->startThread();
SVO_INFO_STREAM("NanoSlam initialized");
}
FrameHandlerBase::
~FrameHandlerBase()
FrameHandlerBase::~FrameHandlerBase()
{
SVO_INFO_STREAM("SVO destructor invoked.");
delete depth_filter_;
SVO_INFO_STREAM("SVO destructed.");
}
bool FrameHandlerBase::
startFrameProcessingCommon()
bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
{
if(set_start_)
{
resetAll();
stage_ = FIRST_FRAME;
stage_ = STAGE_FIRST_FRAME;
}
if(stage_ == PAUSED)
if(stage_ == STAGE_PAUSED)
return false;
SVO_LOG(timestamp);
SVO_DEBUG_STREAM("New Frame");
SVO_START_TIMER("tot_time");
timer_.start();
@@ -110,15 +102,17 @@ startFrameProcessingCommon()
return true;
}
int FrameHandlerBase::
finishFrameProcessingCommon(size_t update_id, UpdateResult dropout, size_t num_observations)
int FrameHandlerBase::finishFrameProcessingCommon(
const size_t update_id,
const UpdateResult dropout,
const size_t num_observations)
{
SVO_DEBUG_STREAM("Frame: "<<update_id<<"\t fps-avg = "<< 1.0/acc_frame_timings_.getMean()<<"\t nObs = "<<acc_num_obs_.getMean());
SVO_LOG(dropout);
// save processing time to calculate fps
acc_frame_timings_.push_back(timer_.stop());
if(stage_ == DEFAULT_FRAME)
if(stage_ == STAGE_DEFAULT_FRAME)
acc_num_obs_.push_back(num_observations);
num_obs_last_ = num_observations;
SVO_STOP_TIMER("tot_time");
@@ -133,43 +127,40 @@ finishFrameProcessingCommon(size_t update_id, UpdateResult dropout, size_t num_o
#endif
// reset if failure or requested
if(dropout == FAILURE || set_reset_)
if(dropout == RESULT_FAILURE || set_reset_)
resetAll();
return 0;
}
void FrameHandlerBase::
resetCommon()
void FrameHandlerBase::resetCommon()
{
map_.reset();
stage_ = PAUSED;
stage_ = STAGE_PAUSED;
set_reset_ = false;
set_start_ = false;
tracking_quality_ = BAD;
depth_filter_->reset();
tracking_quality_ = TRACKING_BAD;
num_obs_last_ = 0;
SVO_INFO_STREAM("RESET");
}
void FrameHandlerBase::
setTrackingQuality(const size_t num_observations)
void FrameHandlerBase::setTrackingQuality(const size_t num_observations)
{
tracking_quality_ = GOOD;
tracking_quality_ = TRACKING_GOOD;
if(num_observations < Config::qualityMinFts())
{
ROS_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!");
tracking_quality_ = BAD;
tracking_quality_ = TRACKING_BAD;
}
const int feature_drop = static_cast<int>(num_obs_last_) - num_observations;
if(feature_drop > Config::qualityMaxFtsDrop())
{
ROS_WARN_STREAM("Lost "<< feature_drop <<" features!");
tracking_quality_ = BAD;
tracking_quality_ = TRACKING_BAD;
}
if(num_observations < 20)
{
ROS_ERROR_STREAM("Tracking less than 20 features!");
tracking_quality_ = INSUFFICIENT;
tracking_quality_ = TRACKING_INSUFFICIENT;
}
}
@@ -178,8 +169,10 @@ bool ptLastOptimComparator(Point* lhs, Point* rhs)
return (lhs->last_structure_optim_ < rhs->last_structure_optim_);
}
void FrameHandlerBase::
optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter)
void FrameHandlerBase::optimizeStructure(
FramePtr frame,
size_t max_n_pts,
int max_iter)
{
deque<Point*> pts;
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
svo/src/frame_handler_mono.cpp
+46 -48
View File
@@ -30,22 +30,27 @@
namespace svo {
FrameHandlerMono::
FrameHandlerMono(vk::AbstractCamera* cam) :
FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam) :
FrameHandlerBase(),
cam_(cam)
cam_(cam),
reprojector_(cam_, map_),
depth_filter_(NULL)
{
reprojection_grid_.initialize(cam_);
// create depth filter and set callback
feature_detection::DetectorPtr feature_detector(new feature_detection::FastDetector());
DepthFilter::callback_t depth_filter_cb = boost::bind(&MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2);
depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb);
depth_filter_->startThread();
}
FrameHandlerMono::
~FrameHandlerMono()
{}
void FrameHandlerMono::
addImage(const cv::Mat& img, double timestamp)
FrameHandlerMono::~FrameHandlerMono()
{
if(!startFrameProcessingCommon())
delete depth_filter_;
}
void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)
{
if(!startFrameProcessingCommon(timestamp))
return;
// some cleanup from last iteration, can't do before because of visualization
@@ -58,12 +63,12 @@ addImage(const cv::Mat& img, double timestamp)
SVO_STOP_TIMER("pyramid_creation");
// process frame
UpdateResult res = FAILURE;
if(stage_ == DEFAULT_FRAME)
UpdateResult res = RESULT_FAILURE;
if(stage_ == STAGE_DEFAULT_FRAME)
res = processFrame();
else if(stage_ == SECOND_FRAME)
else if(stage_ == STAGE_SECOND_FRAME)
res = processSecondFrame();
else if(stage_ == FIRST_FRAME)
else if(stage_ == STAGE_FIRST_FRAME)
res = processFirstFrame();
// set last frame
@@ -74,37 +79,34 @@ addImage(const cv::Mat& img, double timestamp)
finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
}
void FrameHandlerMono::
setFirstFrame(const FramePtr& first_frame)
void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
{
resetAll();
last_frame_ = first_frame;
last_frame_->setKeyframe();
map_.addKeyframe(last_frame_);
stage_ = DEFAULT_FRAME;
stage_ = STAGE_DEFAULT_FRAME;
}
FrameHandlerMono::UpdateResult FrameHandlerMono::
processFirstFrame()
FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
{
new_frame_->T_f_w_ = T_f_w_init_;
if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
return NO_KEYFRAME;
return RESULT_NO_KEYFRAME;
new_frame_->setKeyframe();
map_.addKeyframe(new_frame_);
stage_ = SECOND_FRAME;
stage_ = STAGE_SECOND_FRAME;
SVO_INFO_STREAM("Init: Selected first frame.");
return IS_KEYFRAME;
return RESULT_IS_KEYFRAME;
}
FrameHandlerMono::UpdateResult FrameHandlerMono::
processSecondFrame()
FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
{
initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_);
if(res == initialization::FAILURE)
return FAILURE;
return RESULT_FAILURE;
else if(res == initialization::NO_KEYFRAME)
return NO_KEYFRAME;
return RESULT_NO_KEYFRAME;
// two-frame bundle adjustment
#ifdef USE_BUNDLE_ADJUSTMENT
@@ -118,14 +120,13 @@ processSecondFrame()
// add frame to map
new_frame_->setKeyframe();
map_.addKeyframe(new_frame_);
stage_ = DEFAULT_FRAME;
stage_ = STAGE_DEFAULT_FRAME;
klt_homography_init_.reset();
SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map.");
return IS_KEYFRAME;
return RESULT_IS_KEYFRAME;
}
FrameHandlerMono::UpdateResult FrameHandlerMono::
processFrame()
FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
{
// Set initial pose TODO use prior
new_frame_->T_f_w_ = last_frame_->T_f_w_;
@@ -141,16 +142,15 @@ 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_ERROR_STREAM("Not enough matched features.");
return FAILURE;
return RESULT_FAILURE;
}
// pose optimization
@@ -165,7 +165,7 @@ processFrame()
SVO_DEBUG_STREAM("PoseOptimizer:\t ErrInit = "<<sfba_error_init<<"px\t thresh = "<<sfba_thresh);
SVO_DEBUG_STREAM("PoseOptimizer:\t ErrFin. = "<<sfba_error_final<<"px\t nObsFin. = "<<sfba_n_edges_final);
if(sfba_n_edges_final < 10)
return FAILURE;
return RESULT_FAILURE;
// structure optimization
SVO_START_TIMER("point_optimizer");
@@ -174,14 +174,14 @@ processFrame()
// select keyframe
setTrackingQuality(sfba_n_edges_final);
if(tracking_quality_ == INSUFFICIENT)
return FAILURE;
if(tracking_quality_ == TRACKING_INSUFFICIENT)
return RESULT_FAILURE;
double depth_mean, depth_min;
frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
if(!needNewKf(depth_mean) || tracking_quality_ == BAD)
if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)
{
depth_filter_->addFrame(new_frame_);
return NO_KEYFRAME;
return RESULT_NO_KEYFRAME;
}
// new keyframe selected
@@ -224,21 +224,20 @@ processFrame()
new_frame_->setKeyframe();
map_.addKeyframe(new_frame_);
return IS_KEYFRAME;
return RESULT_IS_KEYFRAME;
}
void FrameHandlerMono::
resetAll()
void FrameHandlerMono::resetAll()
{
resetCommon();
last_frame_.reset();
new_frame_.reset();
core_kfs_.clear();
overlap_kfs_.clear();
depth_filter_->reset();
}
bool FrameHandlerMono::
needNewKf(double scene_depth_mean)
bool FrameHandlerMono::needNewKf(double scene_depth_mean)
{
for(auto it=overlap_kfs_.begin(), ite=overlap_kfs_.end(); it!=ite; ++it)
{
@@ -251,8 +250,7 @@ needNewKf(double scene_depth_mean)
return true;
}
void FrameHandlerMono::
setCoreKfs(size_t n_closest)
void FrameHandlerMono::setCoreKfs(size_t n_closest)
{
size_t n = min(n_closest, overlap_kfs_.size()-1);
std::partial_sort(overlap_kfs_.begin(), overlap_kfs_.begin()+n, overlap_kfs_.end(),
svo/src/reprojection.cpp → svo/src/reprojector.cpp
+62 -65
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,60 +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));
@@ -99,64 +98,63 @@ 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++;
}
}
SVO_STOP_TIMER("reproject_kfs");
// 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;
}
}
++it;
}
}
} // unlock the mutex when out of scope
SVO_STOP_TIMER("reproject_candidates");
// 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.
n_matches=0; n_trials=0;
for(size_t i=0; i<grid.cells.size(); ++i)
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)
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::TYPE_DELETED)
{
@@ -165,14 +163,16 @@ bool reprojectCell(
}
int feature_level;
bool found_match = matcher::findMatchDirect(*it->pt, *frame, it->px, feature_level);
bool found_match = true;
if(options_.find_match_direct)
found_match = matcher::findMatchDirect(*it->pt, *frame, it->px, feature_level);
if(!found_match)
{
it->pt->n_failed_reproj_++;
if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
map.safeDeletePoint(it->pt);
map_.safeDeletePoint(it->pt);
if(it->pt->type_ == Point::TYPE_CANDIDATE && it->pt->n_failed_reproj_ > 30)
candidate_points.deleteCandidatePoint(it->pt);
map_.point_candidates_.deleteCandidatePoint(it->pt);
it = cell.erase(it);
continue;
}
@@ -195,20 +195,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
svo/src/sparse_img_align.cpp
+25 -33
View File
@@ -26,12 +26,12 @@
namespace svo {
SparseImgAlign::
SparseImgAlign(int max_level, int min_level, int n_iter,
Method method, bool display, bool verbose) :
display_(display),
max_level_(max_level),
min_level_(min_level)
SparseImgAlign::SparseImgAlign(
int max_level, int min_level, int n_iter,
Method method, bool display, bool verbose) :
display_(display),
max_level_(max_level),
min_level_(min_level)
{
n_iter_ = n_iter;
n_iter_init_ = n_iter_;
@@ -40,8 +40,7 @@ SparseImgAlign(int max_level, int min_level, int n_iter,
eps_ = 0.000001;
}
size_t SparseImgAlign::
run(FramePtr ref_frame, FramePtr cur_frame)
size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
{
reset();
@@ -55,7 +54,7 @@ run(FramePtr ref_frame, FramePtr cur_frame)
cur_frame_ = cur_frame;
ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);
jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_);
visible_fts_.resize(ref_patch_cache_.rows, false);
visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?
SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse());
@@ -74,9 +73,7 @@ run(FramePtr ref_frame, FramePtr cur_frame)
}
// TODO take jacobian from Frame Class!
void
frameJac(const Vector3d & xyz,
Matrix<double,2,6> & J)
void frameJac(const Vector3d & xyz, Matrix<double,2,6> & J)
{
const double x = xyz[0];
const double y = xyz[1];
@@ -98,8 +95,7 @@ frameJac(const Vector3d & xyz,
J(1,5) = x*z_inv; // x/z
}
void SparseImgAlign::
precomputeReferencePatches()
void SparseImgAlign::precomputeReferencePatches()
{
const int border = patch_halfsize_+1;
const cv::Mat& ref_img = ref_frame_->img_pyr_.at(level_);
@@ -155,22 +151,23 @@ precomputeReferencePatches()
// cache the jacobian
jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter) =
(dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<<level_));
- (dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<<level_));
}
}
}
have_ref_patch_cache_ = true;
}
double SparseImgAlign::
computeResiduals(const SE3& T_cur_from_ref, bool linearize_system, bool compute_weight_scale)
double SparseImgAlign::computeResiduals(
const SE3& T_cur_from_ref,
bool linearize_system,
bool compute_weight_scale)
{
// Warp the (cur)rent image such that it aligns with the (ref)erence image
const cv::Mat& ref_img = ref_frame_->img_pyr_.at(level_);
const cv::Mat& cur_img = cur_frame_->img_pyr_.at(level_);
if(linearize_system && display_)
resimg_ = cv::Mat(ref_img.size(), CV_32F, cv::Scalar(0));
resimg_ = cv::Mat(cur_img.size(), CV_32F, cv::Scalar(0));
if(have_ref_patch_cache_ == false)
precomputeReferencePatches();
@@ -179,7 +176,7 @@ computeResiduals(const SE3& T_cur_from_ref, bool linearize_system, bool compute_
std::vector<float> errors;
if(compute_weight_scale)
errors.reserve(visible_fts_.size());
const int stride = ref_img.cols;
const int stride = cur_img.cols;
const int border = patch_halfsize_+1;
const float scale = 1.0f/(1<<level_);
const Vector3d ref_pos(ref_frame_->pos());
@@ -259,8 +256,7 @@ computeResiduals(const SE3& T_cur_from_ref, bool linearize_system, bool compute_
return chi2/n_meas_;
}
int SparseImgAlign::
solve()
int SparseImgAlign::solve()
{
x_ = H_.ldlt().solve(Jres_);
if((bool) std::isnan((double) x_[0]))
@@ -268,26 +264,22 @@ solve()
return 1;
}
void SparseImgAlign::
update(const ModelType& T_curold_from_ref, ModelType& T_curnew_from_ref)
void SparseImgAlign::update(
const ModelType& T_curold_from_ref,
ModelType& T_curnew_from_ref)
{
// the jacobian is computed in the coordinate frame of the reference image
// T_ref_from_curnew = T_ref_from_curold * SE3::exp(x_).inverse();
// but our state is in the reference frame of the new image, thus using (AB)^-1 = B^-1*A^-1
T_curnew_from_ref = SE3::exp(x_) * T_curold_from_ref;
T_curnew_from_ref = T_curold_from_ref * SE3::exp(-x_);
}
void SparseImgAlign::
startIteration()
void SparseImgAlign::startIteration()
{}
void SparseImgAlign::
finishIteration()
void SparseImgAlign::finishIteration()
{
if(display_)
{
cv::namedWindow("residuals", CV_WINDOW_AUTOSIZE);
cv::imshow("residuals", resimg_*4);
cv::imshow("residuals", resimg_*10);
cv::waitKey(0);
}
}
svo/test/test_depth_filter.cpp
+6 -4
View File
@@ -32,6 +32,7 @@
#include <svo/feature.h>
#include <svo/point.h>
#include <svo/config.h>
#include "test_utils.h"
namespace {
@@ -151,14 +152,15 @@ void DepthFilterTest::testReconstruction(
}
// trace error
std::string trace_name(std::string(TEST_TRACE_DIR) + "/depth_filter_" + experiment_name + ".txt");
std::string trace_dir(svo::test_utils::getTraceDir());
std::string trace_name(trace_dir + "/depth_filter_" + experiment_name + ".txt");
std::ofstream ofs(trace_name.c_str());
for(std::list<ConvergedSeed>::iterator i=results_.begin(); i!=results_.end(); ++i)
ofs << i->x_ << ", " << i->y_ << ", " << fabs(i->error_) << std::endl;
ofs.close();
// trace convergence rate
trace_name = std::string(TEST_TRACE_DIR) + "/depth_filter_" + experiment_name + "_convergence.txt";
trace_name = trace_dir + "/depth_filter_" + experiment_name + "_convergence.txt";
ofs.open(trace_name.c_str());
for(std::list<size_t>::iterator it=n_converged_per_iteration.begin();
it!=n_converged_per_iteration.end(); ++it)
@@ -166,7 +168,7 @@ void DepthFilterTest::testReconstruction(
ofs.close();
// write ply file for pointcloud visualization in Meshlab
trace_name = std::string(TEST_TRACE_DIR) + "/depth_filter_" + experiment_name + ".ply";
trace_name = trace_dir + "/depth_filter_" + experiment_name + ".ply";
ofs.open(trace_name.c_str());
ofs << "ply" << std::endl
<< "format ascii 1.0" << std::endl
@@ -193,7 +195,7 @@ void DepthFilterTest::testReconstruction(
int main(int argc, char** argv)
{
DepthFilterTest test;
std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
std::string experiment_name("synth_fast_2d");
test.testReconstruction(dataset_dir, experiment_name);
return 0;
svo/test/test_feature_alignment.cpp
+3 -1
View File
@@ -19,6 +19,7 @@
#include <svo/feature_alignment.h>
#include <vikit/timer.h>
#include <vikit/aligned_mem.h>
#include "test_utils.h"
#if __ARM_NEON__
#include <arm_neon.h>
#endif
@@ -52,7 +53,8 @@ void generateRefPatchNoWarpInterpolate(const cv::Mat& img, const Vector2d& px,
int main(int argc, char **argv)
{
std::string img_name(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1/img/frame_000001_0.png");
std::string img_name(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1/img/frame_000001_0.png");
printf("Loading image '%s'\n", img_name.c_str());
cv::Mat img(cv::imread(img_name, 0));
assert(img.type() == CV_8UC1);
svo/test/test_feature_detection.cpp
+2 -1
View File
@@ -23,6 +23,7 @@
#include <vikit/vision.h>
#include <vikit/abstract_camera.h>
#include <vikit/atan_camera.h>
#include "test_utils.h"
namespace {
@@ -31,7 +32,7 @@ using namespace std;
void testCornerDetector()
{
std::string img_name(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1/img/frame_000001_0.png");
std::string img_name(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1/img/frame_000001_0.png");
printf("Loading image '%s'\n", img_name.c_str());
cv::Mat img(cv::imread(img_name, 0));
assert(img.type() == CV_8UC1 && !img.empty());
svo/test/test_matcher.cpp
+11 -9
View File
@@ -22,6 +22,7 @@
#include <svo/frame.h>
#include <svo/config.h>
#include <svo/feature.h>
#include "test_utils.h"
namespace {
@@ -33,7 +34,7 @@ class MatcherTest {
cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
// load images
std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
std::string img_name(dataset_dir+"/img/frame_000071_0.png");
printf("Loading image '%s'\n", img_name.c_str());
cv::Mat img_ref(cv::imread(img_name, 0));
@@ -112,31 +113,32 @@ void MatcherTest::testEpipolarSearchFullImg()
}
// compute mean, median and variance of error in converged area
printf("n converged: \t %zu (ref: 194472)\n", n_converged);
printf("mean error: \t %f (ref: 1.0848)\n", sum_error*100/n_converged);
printf("n converged: \t %zu mm (ref: 169936)\n", n_converged);
printf("mean error: \t %f mm (ref: 1.912410)\n", sum_error*100/n_converged);
std::vector<double>::iterator it = errors.begin()+0.5*errors.size();
std::nth_element(errors.begin(), it, errors.end());
printf("50-percentile: \t %f (ref: 0.1343)\n", *it*100);
printf("50-percentile: \t %f mm (ref: 1.462241)\n", *it*100);
it = errors.begin()+0.8*errors.size();
std::nth_element(errors.begin(), it, errors.end());
printf("80-percentile: \t %f (ref: 0.3151)\n", *it*100);
printf("80-percentile: \t %f mm (ref: 2.620957)\n", *it*100);
it = errors.begin()+0.95*errors.size();
std::nth_element(errors.begin(), it, errors.end());
printf("95-percentile: \t %f (ref: 1.3753)\n", *it*100);
printf("95-percentile: \t %f mm (ref: 4.154051)\n", *it*100);
// save results to file
std::ofstream output_stream;
std::string output_filename(std::string(TEST_TRACE_DIR) + "/depthmap.bin");
std::string trace_dir(svo::test_utils::getTraceDir());
std::string output_filename(trace_dir + "/depthmap.bin");
output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
output_stream.write((const char *)depthmap.data, depthmap.rows*depthmap.cols*sizeof(float));
output_stream.close();
output_filename.assign(std::string(TEST_TRACE_DIR) + "/depthmap_error.bin");
output_filename.assign(trace_dir + "/depthmap_error.bin");
output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
output_stream.write((const char *)error_map.data, error_map.rows*error_map.cols*sizeof(float));
output_stream.close();
output_filename.assign(std::string(TEST_TRACE_DIR) + "/depthmap_mask.bin");
output_filename.assign(trace_dir + "/depthmap_mask.bin");
output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
output_stream.write((const char *)mask.data, mask.rows*mask.cols*sizeof(uint8_t));
output_stream.close();
svo/test/test_pipeline.cpp
+6 -6
View File
@@ -28,6 +28,7 @@
#include <opencv2/opencv.hpp>
#include <sophus/se3.h>
#include <iostream>
#include "test_utils.h"
namespace svo {
@@ -44,8 +45,6 @@ public:
BenchmarkNode::BenchmarkNode()
{
Config::traceName() = "benchmark";
Config::traceDir() = std::string(TEST_TRACE_DIR)+"/..";
cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
vo_ = new svo::FrameHandlerMono(cam_);
vo_->start();
@@ -62,8 +61,9 @@ void BenchmarkNode::runFromFolder()
for(int img_id = 1; img_id < 310; ++img_id)
{
// load image
std::stringstream ss;
ss << std::string(TEST_DATA_DIR) << "/flying_room_1_rig_1/img/frame_" << std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png";
std::stringstream ss;
ss << svo::test_utils::getDatasetDir() << "/flying_room_1_rig_1/img/frame_"
<< std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png";
if(img_id == 1)
std::cout << "reading image " << ss.str() << std::endl;
cv::Mat img(cv::imread(ss.str().c_str(), 0));
@@ -75,8 +75,8 @@ void BenchmarkNode::runFromFolder()
// display tracking quality
if(vo_->lastFrame() != NULL)
std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t"
<< "#Features: " << vo_->lastNumObservations() << " \t"
<< "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";
<< "#Features: " << vo_->lastNumObservations() << " \t"
<< "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";
}
}
svo/test/test_pose_optimizer.cpp
+2 -1
View File
@@ -25,6 +25,7 @@
#include <svo/config.h>
#include <svo/feature_detection.h>
#include <svo/pose_optimizer.h>
#include "test_utils.h"
namespace {
@@ -39,7 +40,7 @@ class PoseOptimizerTest {
cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
// load image
std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
std::string img_name(dataset_dir+"/img/frame_000001_0.png");
printf("Loading image '%s'\n", img_name.c_str());
cv::Mat img(cv::imread(img_name, 0));
svo/test/test_sparse_img_align.cpp
+5 -3
View File
@@ -26,6 +26,7 @@
#include <svo/point.h>
#include <svo/feature.h>
#include <svo/config.h>
#include "test_utils.h"
namespace {
@@ -68,7 +69,8 @@ void SparseImgAlignTest::testSequence(
std::list<double> translation_error;
Sophus::SE3 T_prev_w, T_prevgt_w;
std::string trace_name(std::string(TEST_TRACE_DIR) + "/sparse_img_align_" + experiment_name + "_trans_estimate.txt");
std::string trace_dir(svo::test_utils::getTraceDir());
std::string trace_name(trace_dir + "/sparse_img_align_" + experiment_name + "_trans_estimate.txt");
std::ofstream ofs(trace_name.c_str());
for(int i=0; iter != sequence.end() && i<30; ++iter, ++i)
{
@@ -142,7 +144,7 @@ void SparseImgAlignTest::testSequence(
ofs.close();
// trace error
trace_name = std::string(TEST_TRACE_DIR) + "/sparse_img_align_" + experiment_name + "_trans_error.txt";
trace_name = trace_dir + "/sparse_img_align_" + experiment_name + "_trans_error.txt";
ofs.open(trace_name.c_str());
for(std::list<double>::iterator it=translation_error.begin(); it!=translation_error.end(); ++it)
ofs << *it << std::endl;
@@ -155,7 +157,7 @@ void SparseImgAlignTest::testSequence(
int main(int argc, char** argv)
{
std::string experiment_name("flying_room_1_rig_1_fast_minlev0");
std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
svo::feature_detection::FastDetector detector;
svo::Config::triangMinCornerScore() = 20;
svo::Config::kltMinLevel() = 0;
svo/test/test_utils.h
+47
View File
@@ -0,0 +1,47 @@
// 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 TEST_UTILS_H_
#define TEST_UTILS_H_
#include <string.h>
#include <cstdlib> // for getenv
#include <ros/package.h>
#include <vikit/params_helper.h>
namespace svo {
namespace test_utils {
std::string getDatasetDir()
{
std::string dataset_dir(ros::package::getPath("svo")+"/test/data");
const char* env_dir = std::getenv("SVO_DATASET_DIR");
if(env_dir != NULL)
dataset_dir = std::string(env_dir);
return dataset_dir;
}
std::string getTraceDir()
{
std::string default_dir(ros::package::getPath("svo")+"/test/results");
return vk::getParam<std::string>("svo/trace_dir", default_dir);
}
} // namespace test_utils
} // namespace svo
#endif // TEST_UTILS_H_
svo_analysis/README.md
+27
View File
@@ -0,0 +1,27 @@
## SVO Analysis
#### Download Test Data
Create a folder where you will store all datasets, e.g.:
mkdir Datasets
In your `.bashrc` script, create a new environment variable pointing to the dataset folder:
export SVO_DATASET_DIR=${HOME}/Datasets
Source your new `.bashrc` script, go to the new dataset folder and download the test data:
source `~/.bashrc`
cd ${SVO_DATASET_DIR}
wget http://rpg.ifi.uzh.ch/datasets/flying_room_1_rig_1.tar.gz -O - | tar -xz
#### Run Benchmark
In SVO you need to set the `TRACE` flag to `TRUE` and recompile SVO.
Afterwards run
rosrun svo_analysis benchmark.py flying_room_1_fast
The script will create a new folder in `svo_analysis/results/`, run svo on the dataset and save the tracefile in this folder and at the end it will run some scripts to generate plots from the tracefile.
svo_analysis/experiments/.gitignore
+2
View File
@@ -0,0 +1,2 @@
*.yaml
!.gitignore
svo_analysis/experiments/asl2_acc.yaml
+2 -1
View File
@@ -1,2 +1,3 @@
dataset: asl_vicon_d2
param_settings: vo_accurate
param_settings: vo_accurate
experiment_label: 'svo acc'
svo_analysis/experiments/asl2_fast.yaml
+2 -1
View File
@@ -1,2 +1,3 @@
dataset: asl_vicon_d2
param_settings: vo_fast
param_settings: vo_fast
experiment_label: 'svo fast'
svo_analysis/experiments/flying_room_1_fast.yaml
+2 -1
View File
@@ -1,2 +1,3 @@
dataset: flying_room_1_rig_1
param_settings: vo_fast
param_settings: vo_fast
experiment_label: 'svo fast'
svo_analysis/scripts/benchmark.py
+6 -5
View File
@@ -10,9 +10,9 @@ import yaml
import rospkg
import argparse
import time
import svo_analysis.analyse_trajectory as analyse_trajectory
import vikit_py.cpu_info as cpu_info
import vikit_py.ros_node as ros_node
import evaluate
if __name__=="__main__":
@@ -23,6 +23,8 @@ if __name__=="__main__":
parser.add_argument('experiment_file', help='experiment file in svo_analysis/experiments folder')
parser.add_argument('--name', help='experiment name')
parser.add_argument('--evaluate', help='evaluate tracefile after running SVO', action='store_true')
parser.add_argument('--version', help='version of svo to evaluate', default='svo_ros')
parser.add_argument('--executable', help='the executable to be called', default='benchmark')
args = parser.parse_args()
# load experiment parameters
@@ -33,7 +35,7 @@ if __name__=="__main__":
expParams['platform'] = cpu_info.get_cpu_info()
expParams['experiment_name'] = args.name
if(expParams['experiment_name'] == None):
expParams['experiment_name'] = expParams['time']+'_svo_' + args.experiment_file
expParams['experiment_name'] = expParams['time']+'_'+args.version+'_' + args.experiment_file
# load dataset parameters
expParams['dataset_directory'] = os.path.join(rospkg.RosPack().get_path('Datasets'), expParams['dataset'])
@@ -63,12 +65,11 @@ if __name__=="__main__":
outfile.write(yaml.dump(params, default_flow_style=False))
# execute ros node
node = ros_node.RosNode('svo_ros','benchmark')
node = ros_node.RosNode(args.version, args.executable)
node.run(params)
# TODO: check if it is a synthetic dataset
# plot trajectory error
analyse_trajectory.analyse_trajectory(params['trace_dir'])
evaluate.evaluate_dataset(params['trace_dir'])
svo_analysis/scripts/comparison.py
Regular → Executable
+72 -88
View File
@@ -1,32 +1,32 @@
# -*- coding: utf-8 -*-
#!/usr/bin/python
import os
import sys
import time
import rospkg
import numpy as np
import matplotlib.pyplot as plt
import yaml
import argparse
from matplotlib import rc
# tell matplotlib to use latex font
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
from mpl_toolkits.axes_grid1.inset_locator import zoomed_inset_axes
from mpl_toolkits.axes_grid1.inset_locator import mark_inset
def plotTrajectory(ax, filename, label, color, linewidth):
def plot_trajectory(ax, filename, label, color, linewidth):
file = open(filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
trajectory = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
ax.plot(trajectory[:,1], trajectory[:,2], label=label, color=color, linewidth=linewidth)
if __name__ == '__main__':
def compare_results(experiments, results_dir, comparison_dir,
plot_scale_drift = False):
results_dir = '../results'
experiments = ['20130911_2113_nslam_i7_asl2_acc',
'20130911_2229_nslam_i7_asl2_fast',
'20130906_2149_ptam_i7_asl2']
# tell matplotlib to use latex font
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
# ------------------------------------------------------------------------------
# position error
fig_poserr = plt.figure(figsize=(8,6))
@@ -37,28 +37,25 @@ if __name__ == '__main__':
for exp in experiments:
# load dataset parameters
params_stream = open(results_dir+'/'+exp+'/params.yaml')
params_stream = open(os.path.join(results_dir, exp, 'params.yaml'))
params = yaml.load(params_stream)
# plot translation error
trans_error_filename = results_dir+'/'+exp+'/translation_error.txt'
file = open(trans_error_filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
trans_error = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
trans_error = np.loadtxt(os.path.join(results_dir, exp, 'translation_error.txt'))
trans_error[:,0] = trans_error[:,0]-trans_error[0,0]
ax_poserr_x.plot(trans_error[:,0], trans_error[:,1], label=params['experiment_label'])
ax_poserr_y.plot(trans_error[:,0], trans_error[:,2])
ax_poserr_z.plot(trans_error[:,0], trans_error[:,3])
ax_poserr_x.set_xlim([0, trans_error[-1,0]+4])
ax_poserr_y.set_xlim([0, trans_error[-1,0]+4])
ax_poserr_y.set_xlim([0, trans_error[-1,0]+4])
ax_poserr_z.set_xlim([0, trans_error[-1,0]+4])
ax_poserr_x.legend(bbox_to_anchor=[0, 0], loc='lower left', ncol=3)
ax_poserr_x.grid()
ax_poserr_y.grid()
ax_poserr_z.grid()
fig_poserr.tight_layout()
fig_poserr.savefig('../results/translation_error.pdf')
fig_poserr.savefig(os.path.join(comparison_dir, 'translation_error.pdf'))
# ------------------------------------------------------------------------------
# orientation error
@@ -70,15 +67,12 @@ if __name__ == '__main__':
for exp in experiments:
# load dataset parameters
params_stream = open(results_dir+'/'+exp+'/params.yaml')
params_stream = open(os.path.join(results_dir, exp, 'params.yaml'))
params = yaml.load(params_stream)
# plot translation error
rot_error_filename = results_dir+'/'+exp+'/orientation_error.txt'
file = open(rot_error_filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
rot_error = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
rot_error = np.loadtxt(os.path.join(results_dir, exp, 'orientation_error.txt'))
rot_error[:,0] = rot_error[:,0]-rot_error[0,0]
ax_roterr_r.plot(rot_error[:,0], rot_error[:,3], label=params['experiment_label'])
ax_roterr_p.plot(rot_error[:,0], rot_error[:,2])
ax_roterr_y.plot(rot_error[:,0], rot_error[:,1])
@@ -91,76 +85,66 @@ if __name__ == '__main__':
ax_roterr_p.grid()
ax_roterr_y.grid()
fig_roterr.tight_layout()
fig_roterr.savefig('../results/orientation_error.pdf')
fig_roterr.savefig(os.path.join(comparison_dir, 'orientation_error.pdf'))
# ------------------------------------------------------------------------------
# scale error
fig_scale = plt.figure(figsize=(8,2.5))
ax_scale = fig_scale.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]')
for exp in experiments:
if plot_scale_drift:
fig_scale = plt.figure(figsize=(8,2.5))
ax_scale = fig_scale.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]')
# load dataset parameters
params_stream = open(results_dir+'/'+exp+'/params.yaml')
params = yaml.load(params_stream)
for exp in experiments:
# load dataset parameters
params = yaml.load(open(os.path.join(results_dir, exp, 'params.yaml')))
# plot translation error
scale_drift = open(os.path.join(results_dir, exp, 'scale_drift.txt'))
scale_drift[:,0] = scale_drift[:,0]-scale_drift[0,0]
ax_scale.plot(scale_drift[:,0], scale_drift[:,1], label=params['experiment_label'])
# plot translation error
scale_drift_filename = results_dir+'/'+exp+'/scale_drift.txt'
file = open(scale_drift_filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
scale_drift = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
ax_scale.plot(scale_drift[:,0], scale_drift[:,1], label=params['experiment_label'])
ax_scale.set_xlim([0, rot_error[-1,0]+4])
ax_scale.legend(bbox_to_anchor=[0, 1], loc='upper left', ncol=3)
ax_scale.grid()
fig_scale.tight_layout()
fig_scale.savefig('../results/scale_drift.pdf')
ax_scale.set_xlim([0, rot_error[-1,0]+4])
ax_scale.legend(bbox_to_anchor=[0, 1], loc='upper left', ncol=3)
ax_scale.grid()
fig_scale.tight_layout()
fig_scale.savefig(os.path.join(comparison_dir, 'scale_drift.pdf'))
# ------------------------------------------------------------------------------
# trajectory
fig_traj = plt.figure(figsize=(8,4.8))
ax_traj = fig_traj.add_subplot(111, xlabel='x [m]', ylabel='y [m]', aspect='equal', xlim=[-3.1, 4], ylim=[-1.5, 2.6])
plotTrajectory(ax_traj, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5)
plotTrajectory(ax_traj, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1)
plotTrajectory(ax_traj, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1)
# show detail:
axins = zoomed_inset_axes(ax_traj, 5.0, loc=1) # zoom = 6
plotTrajectory(axins, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5)
plotTrajectory(axins, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1)
plotTrajectory(axins, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1)
axins.set_xlim(1.2, 1.70)
axins.set_ylim(-0.8, -0.3)
axins.patch.set_facecolor('0.9')
plt.xticks(visible=False)
plt.yticks(visible=False)
mark_inset(ax_traj, axins, loc1=2, loc2=4, fc="none", ec='b')
plt.draw()
plt.show()
ax_traj.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=3)
ax_traj.grid()
fig_traj.tight_layout()
fig_traj.savefig('../results/trajectory_asl.pdf')
# fig_traj = plt.figure(figsize=(8,4.8))
# ax_traj = fig_traj.add_subplot(111, xlabel='x [m]', ylabel='y [m]', aspect='equal', xlim=[-3.1, 4], ylim=[-1.5, 2.6])
#
# plotTrajectory(ax_traj, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5)
# plotTrajectory(ax_traj, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1)
# plotTrajectory(ax_traj, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1)
#
# mark_inset(ax_traj, axins, loc1=2, loc2=4, fc="none", ec='b')
# plt.draw()
# plt.show()
# ax_traj.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=3)
# ax_traj.grid()
# fig_traj.tight_layout()
# fig_traj.savefig('../results/trajectory_asl.pdf')
# ------------------------------------------------------------------------------
# number of matches
if __name__ == '__main__':
# plot number of klt tracked and matched points
# fig = plt.figure(figsize=(8,2))
# ax = fig.add_subplot(111, xlabel='time [s]', ylabel='no. features', ylim=[0,220])
# ax.plot(np.arange(len(n_fts_1))/25.0, n_fts_1, 'b-', label='Accurate')
# ax.plot(np.arange(len(n_fts_2))/25.0, n_fts_2, 'g-', label='Fast')
# ax.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=2)
# fig.tight_layout()
# fig.savefig('../results/number_of_features2.pdf', bbox_inches="tight")
#
default_name = time.strftime("%Y%m%d_%H%M", time.localtime())+'_comparison'
parser = argparse.ArgumentParser(description='Compare results.')
parser.add_argument('result_directories', nargs='+', help='list of result directories to compare')
parser.add_argument('--name', help='name of the comparison', default=default_name)
args = parser.parse_args()
# create folder for comparison results
results_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'), 'results')
comparison_dir = os.path.join(results_dir, args.name)
if not os.path.exists(comparison_dir):
os.makedirs(comparison_dir)
# run comparison
compare_results(args.result_directories, results_dir, comparison_dir)
svo_analysis/scripts/evaluate.py
+22 -19
View File
@@ -5,26 +5,19 @@ Created on Sat Aug 10 18:21:47 2013
@author: Christian Forster
"""
import argparse
import os
import csv
import numpy as np
from matplotlib import rc
import rospkg
import argparse
import svo_analysis.analyse_logs as analyse_logs
import svo_analysis.analyse_timing as analyse_timing
import svo_analysis.analyse_trajectory as analyse_trajectory
if __name__=="__main__":
def evaluate_dataset(trace_dir):
# parse command line
parser = argparse.ArgumentParser()
parser.add_argument('--trace_name', help='svo tracefile with timings and logs', default='20131121_0930_px7')
args = parser.parse_args()
# config
results_dir = 'results/'+args.trace_name
trace_file = results_dir+'/'+args.trace_name+'.csv'
# tracefile
data = csv.reader(open(trace_file))
# read tracefile
data = csv.reader(open(os.path.join(trace_dir, 'trace.csv')))
fields = data.next()
D = dict()
for field in fields:
@@ -39,9 +32,19 @@ if __name__=="__main__":
for field, value in D.items():
D[field] = np.array(D[field])
# tell matplotlib to use latex font
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
# generate plots
analyse_logs.analyse_logs(D, trace_dir)
analyse_timing.analyse_timing(D, trace_dir)
analyse_trajectory.analyse_trajectory(trace_dir)
analyse_logs.analyse_logs(D, results_dir+'/'+args.trace_name)
analyse_timing.analyse_timing(D, results_dir+'/'+args.trace_name)
if __name__=="__main__":
parser = argparse.ArgumentParser(description='''
Evaluates tracefiles of SVO and generates the plots.
''')
parser.add_argument('experiment_name', help='directory name of the tracefiles')
args = parser.parse_args()
trace_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'),
'results',
args.experiment_name)
evaluate_dataset(trace_dir)
svo_analysis/src/svo_analysis/analyse_logs.py
+36 -100
View File
@@ -1,16 +1,10 @@
#!/usr/bin/python
import argparse
import csv
import os
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import rc
# tell matplotlib to use latex font
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
def analyse_logs(D, trace_name):
def analyse_logs(D, trace_dir):
# identify measurements which result from normal frames and which from keyframes
is_kf = np.argwhere( (D['dropout'] == 1) & (D['repr_n_mps'] >= 0))
@@ -19,114 +13,56 @@ def analyse_logs(D, trace_name):
n_frames = len(is_frame)
n_kfs = len(is_kf)
n_nokf = len(is_nokf)
fps = 25.0
# set initial time to zero
D['timestamp'] = D['timestamp'] - D['timestamp'][0]
# ----------------------------------------------------------------------------
# plot number of reprojected points
mean_n_reproj_points = np.mean(D['repr_n_mps'][is_frame]);
mean_n_reproj_matches = np.mean(D['repr_n_new_references'][is_frame]);
mean_n_edges_final = np.mean(D['sfba_n_edges_final'][is_frame]);
plt.figure()
plt.plot(np.arange(n_frames), D['repr_n_mps'][is_frame], 'r-', label='reprojected points, avg = %(i).2f'%{'i':mean_n_reproj_points})
plt.plot(np.arange(n_frames), D['repr_n_new_references'][is_frame], 'b-', label='reprojected matches, avg = %(i).2f'%{'i':mean_n_reproj_matches})
plt.legend()
plt.tight_layout()
plt.title('Reprojected points asdf')
plt.savefig(trace_name+'_reprojection.pdf', bbox_inches="tight")
# plot number of triangulations
# plt.figure(3)
# plt.plot(np.arange(n_kfs), D['triang_n_points'][is_kf], 'g-', label='triangulated points, avg %0.2f'%np.mean(D['triang_n_points'][is_kf]))
# plt.plot(np.arange(n_kfs), D['triang_n_refs'][is_kf], 'b-', label='new references, avg %0.2f'%np.mean(D['triang_n_refs'][is_kf]))
# plt.plot(np.arange(n_kfs), D['triang_n_fails'][is_kf], 'r-', label='failures, avg %0.2f'%np.mean(D['triang_n_fails'][is_kf]))
# plt.plot(np.arange(n_kfs), D['triang_n_add_refs'][is_kf], 'm-', label='additional refs, avg %0.2f'%np.mean(D['triang_n_add_refs'][is_kf]))
# plt.legend()
# plt.title('Triangulation')
# plt.savefig(trace_name+'_img_triangulation.png', bbox_inches="tight")
# plot number of klt tracked and matched points
fig = plt.figure(figsize=(8,2))
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='no. features')
ax.plot(np.arange(n_frames)/fps, D['repr_n_new_references'][is_frame], 'b-', label='number of successful matches')
fig = plt.figure(figsize=(8,3))
ax = fig.add_subplot(111, xlabel='time [s]')
ax.plot(D['timestamp'][is_frame], D['repr_n_mps'][is_frame], 'r-',
label='Reprojected Points, avg = %.2f'%mean_n_reproj_points)
ax.plot(D['timestamp'][is_frame], D['repr_n_new_references'][is_frame], 'b-',
label='Feature Matches, avg = %.2f'%mean_n_reproj_matches)
ax.plot(D['timestamp'][is_frame], D['sfba_n_edges_final'][is_frame], 'g-',
label='Points after Optimization, avg = %.2f'%mean_n_edges_final)
ax.set_ylim(bottom=0)
ax.legend(loc='lower right')
fig.tight_layout()
fig.savefig(trace_name+'number_of_features.pdf', bbox_inches="tight")
fig.savefig(os.path.join(trace_dir,'num_reprojected.pdf'), bbox_inches="tight")
# ----------------------------------------------------------------------------
# plot median error before and after pose-optimzation and bundle adjustment
init_error_avg = np.mean(D['sfba_error_init'][is_frame])
opt1_avg = np.mean(D['sfba_error_final'][is_frame])
fig = plt.figure(figsize=(8,2))
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='error [px]')
t_max = n_frames
ax.plot(np.arange(n_frames)[0:t_max]/fps, D['sfba_error_init'][is_frame][0:t_max], 'r-', label='Initial error')
ax.plot(np.arange(n_frames)[0:t_max]/fps, D['sfba_error_final'][is_frame][0:t_max], 'b-', label='Final error')
ax.plot(D['timestamp'][is_frame], D['sfba_error_init'][is_frame], 'r-', label='Initial error')
ax.plot(D['timestamp'][is_frame], D['sfba_error_final'][is_frame], 'b-', label='Final error')
ax.legend(ncol=2)
ax.set_xlim([0, t_max/fps+4])
#ax.set_ylim([0, 0.8])
fig.tight_layout()
fig.savefig(trace_name+'reprojection_error.pdf', bbox_inches="tight")
fig.savefig(os.path.join(trace_dir,'reprojection_error.pdf'), bbox_inches="tight")
print 'average reprojection error improvement: ' + str(init_error_avg - opt1_avg)
# plot number of references
plt.figure()
plt.fill(np.arange(n_frames+1), np.append(D['repr_n_new_references'][is_frame]+D['klt_n_matches'][is_frame],0), 'g-', label='Reprojection')
plt.fill(np.arange(n_frames+1), np.append(D['klt_n_tracked'][is_frame],0), 'b-', label='KLT Matches')
plt.plot(np.arange(n_frames), D['sfba_n_edges_final'][is_frame], 'r-', label='References after Optimization', linewidth=2.0)
plt.legend()
plt.tight_layout()
plt.title('Pose Optimization')
plt.savefig(trace_name+'_img_n_references.pdf', bbox_inches="tight")
# ----------------------------------------------------------------------------
# plot number of candidate points
fig = plt.figure(figsize=(8,3))
ax = fig.add_subplot(111, xlabel='time [s]')
ax.plot(D['timestamp'][is_frame], D['n_candidates'][is_frame], 'r-', label='Candidate Points')
fig.tight_layout()
fig.savefig(os.path.join(trace_dir,'candidate_points.pdf'), bbox_inches="tight")
# plot number of seeds
plt.figure()
has_seed = np.argwhere(D['seeds_n_tot'] >= 0)
plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_tot'][has_seed],0), label='nSeeds', edgecolor='none')
plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_updates'][has_seed]+D['seeds_n_failed_matches'][has_seed],0), label='nFailedMatches', edgecolor='none')
plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_updates'][has_seed],0), label='nUpdates', edgecolor='none')
plt.legend()
plt.tight_layout()
plt.title('Seeds')
plt.savefig(trace_name+'_seeds.pdf', bbox_inches="tight")
plt.figure()
plt.plot(D['n_candidates'])
plt.title('Candidates')
plt.show()
# plt.figure()
# plt.plot())
if __name__=="__main__":
dataset='20131003_1937_nslam_i7_asl2_fast'
n_align_frames = 10
save = True
results_dir = '../results/'+dataset
logfile = results_dir+'/'+dataset+'_0.csv'
# parse command line
parser = argparse.ArgumentParser(description='''
This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
''')
parser.add_argument('--trace_file', help='nanoslam tracefile with timings and logs', default=logfile)
parser.add_argument('--trace_name', help='name of the tracefile, used for saving figures etc.', default=results_dir+'/')
args = parser.parse_args()
# tracefile
data = csv.reader(open(args.trace_file))
fields = data.next()
D = dict()
for field in fields:
D[field] = list()
# fill dictionary with column values
for row in data:
for (field, value) in zip(fields, row):
D[field].append(float(value))
# change dictionary values from list to numpy array for easier manipulation
for field, value in D.items():
D[field] = np.array(D[field])
analyse_logs(D, args.trace_name)
# ----------------------------------------------------------------------------
# plot number of candidate points
fig = plt.figure(figsize=(8,2))
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='px')
ax.plot(D['timestamp'][is_frame], D['sfba_thresh'][is_frame], 'r-', label='Threshold')
fig.tight_layout()
fig.savefig(os.path.join(trace_dir,'optimization_thresh.pdf'), bbox_inches="tight")
svo_analysis/src/svo_analysis/analyse_timing.py
+40 -69
View File
@@ -1,97 +1,68 @@
#!/usr/bin/python
import argparse
import csv
import os
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import rc
def analyse_timing(D, trace_name):
def analyse_timing(D, trace_dir):
# identify measurements which result from normal frames and which from keyframes
is_frame = np.argwhere(D['repr_n_mps'] >= 0)
n_frames = len(is_frame)
# set initial time to zero
D['timestamp'] = D['timestamp'] - D['timestamp'][0]
# ----------------------------------------------------------------------------
# plot total time for frame processing
avg_time = np.mean(D['t_tot_time'][is_frame])*1000;
avg_time = np.mean(D['tot_time'][is_frame])*1000;
fig = plt.figure(figsize=(8, 3))
ax = fig.add_subplot(111, ylabel='processing time [ms]', xlabel='time [s]')
ax.plot(np.arange(n_frames), D['t_tot_time'][is_frame]*1000, 'g-', label='total time [ms]')
ax.plot(np.arange(n_frames), np.ones(n_frames)*avg_time, 'b--', label=str('%(time).1fms mean time' % {'time': avg_time}))
ax.plot(D['timestamp'][is_frame], D['tot_time'][is_frame]*1000, 'g-', label='total time [ms]')
ax.plot(D['timestamp'][is_frame], np.ones(n_frames)*avg_time, 'b--', label=str('%(time).1fms mean time' % {'time': avg_time}))
ax.legend()
fig.tight_layout()
fig.savefig(trace_name+'_img_tot_processing_time.pdf', bbox_inches="tight")
fig.savefig(os.path.join(trace_dir,'timing.pdf'), bbox_inches="tight")
# ----------------------------------------------------------------------------
# plot boxplot
fig = plt.figure(figsize=(6,2))
ax = fig.add_subplot(111, xlabel='Processing time [ms]')
ax.boxplot([
D['t_tot_time'][is_frame]*1000,
D['tot_time'][is_frame]*1000,
# D['t_local_ba'][is_kf]*1000,
D['t_pose_optimizer'][is_frame]*1000 + D['t_point_optimizer'][is_frame]*1000,
D['t_reproject'][is_frame]*1000,
D['t_sparse_img_align'][is_frame]*1000,
D['t_pyramid_creation'][is_frame]*1000
], 0,'', vert=0) #
D['pose_optimizer'][is_frame]*1000 + D['point_optimizer'][is_frame]*1000,
D['reproject'][is_frame]*1000,
D['sparse_img_align'][is_frame]*1000,
D['pyramid_creation'][is_frame]*1000
], 0,'', vert=0)
boxplot_labels = [
r'\textbf{Total Motion Estimation: %2.2fms}' % np.median(D['t_tot_time'][is_frame]*1000),
# 'Local BA (KF only): %.2fms ' % np.median(D['t_local_ba'][is_kf]*1000),
'Refinement: %2.2fms' % np.median(D['t_pose_optimizer'][is_frame]*1000 + D['t_point_optimizer'][is_frame]*1000),
'Feature Alignment: %2.2fms' % np.median(D['t_reproject'][is_frame]*1000),
'Sparse Image Alignment: %2.2fms' % np.median(D['t_sparse_img_align'][is_frame]*1000),
'Pyramid Creation: %2.2fms' % np.median(D['t_pyramid_creation'][is_frame]*1000) ]
r'\textbf{Total Motion Estimation: %2.2fms}' % np.median(D['tot_time'][is_frame]*1000),
# 'Local BA (KF only): %.2fms ' % np.median(D['local_ba'][is_kf]*1000),
'Refinement: %2.2fms' % np.median(D['pose_optimizer'][is_frame]*1000 + D['point_optimizer'][is_frame]*1000),
'Feature Alignment: %2.2fms' % np.median(D['reproject'][is_frame]*1000),
'Sparse Image Alignment: %2.2fms' % np.median(D['sparse_img_align'][is_frame]*1000),
'Pyramid Creation: %2.2fms' % np.median(D['pyramid_creation'][is_frame]*1000) ]
ax.set_yticks(np.arange(len(boxplot_labels))+1)
ax.set_yticklabels(boxplot_labels)
fig.tight_layout()
fig.savefig(trace_name+'_timing_boxplot.pdf', bbox_inches="tight")
fig.savefig(os.path.join(trace_dir,'timing_boxplot.pdf'), bbox_inches="tight")
fig = plt.figure(figsize=(8, 5))
ax = fig.add_subplot(111, ylabel='time [ms]')
ax.plot(D['t_reproject']*1000, 'r', label='reproject')
ax.plot(D['t_reproject_find_pts']*1000, 'g', label='reproject find pts') # ok!
ax.plot(D['t_reproject_best']*1000, 'm', label='reproject best')
ax.plot(D['t_reproject_candidates']*1000, 'b', label='reproject candidates')
ax.legend()
# ----------------------------------------------------------------------------
# plot boxplot reprojection
fig = plt.figure(figsize=(6,2))
ax = fig.add_subplot(111, xlabel='Processing time [ms]')
ax.boxplot([ D['reproject'][is_frame]*1000,
D['feature_align'][is_frame]*1000,
D['reproject_candidates'][is_frame]*1000,
D['reproject_kfs'][is_frame]*1000 ], 0, '', vert=0)
boxplot_labels = [r'\textbf{Total Reprojection: %2.2fms}' % np.median(D['reproject'][is_frame]*1000),
'Feature Alignment: %2.2fms' % np.median(D['feature_align'][is_frame]*1000),
'Reproject Candidates: %2.2fms' % np.median(D['reproject_candidates'][is_frame]*1000),
'Reproject Keyframes: %2.2fms' % np.median(D['reproject_kfs'][is_frame]*1000) ]
ax.set_yticks(np.arange(len(boxplot_labels))+1)
ax.set_yticklabels(boxplot_labels)
fig.tight_layout()
fig.savefig(trace_name+'_reprojection.pdf', bbox_inches="tight")
if __name__=="__main__":
# config
dataset='20131003_1937_nslam_i7_asl2_fast'
results_dir = '../results/'+dataset
tracefile = results_dir + '/'+dataset+'_0.csv'
# parse command line
parser = argparse.ArgumentParser(description='''
This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
''')
parser.add_argument('--trace_file', help='nanoslam tracefile with timings and logs', default=tracefile)
parser.add_argument('--trace_name', help='name of the tracefile, used for saving figures etc.', default=results_dir)
args = parser.parse_args()
results_dir = args.trace_name
# tracefile
data = csv.reader(open(args.trace_file))
fields = data.next()
D = dict()
for field in fields:
D[field] = list()
# fill dictionary with column values
for row in data:
for (field, value) in zip(fields, row):
D[field].append(float(value))
# change dictionary values from list to numpy array for easier manipulation
for field, value in D.items():
D[field] = np.array(D[field])
# tell matplotlib to use latex font
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
fig.savefig(os.path.join(trace_dir,'timing_reprojection.pdf'), bbox_inches="tight")
svo_ros/src/benchmark_node.cpp
+2 -2
View File
@@ -141,7 +141,7 @@ void BenchmarkNode::runBenchmark(const std::string& dataset_dir)
visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, it->timestamp_);
// write pose to tracefile
if(vo_->stage() == svo::FrameHandlerMono::DEFAULT_FRAME)
if(vo_->stage() == svo::FrameHandlerMono::STAGE_DEFAULT_FRAME)
tracePose(vo_->lastFrame()->T_f_w_.inverse(), it->timestamp_);
}
}
@@ -216,7 +216,7 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir)
// Add image to VO
vo_->addImage(img, it->timestamp_);
if(vo_->stage() != svo::FrameHandlerMono::DEFAULT_FRAME)
if(vo_->stage() != svo::FrameHandlerMono::STAGE_DEFAULT_FRAME)
continue;
// Compute pose error and trace to file
svo_ros/src/visualizer.cpp
+2 -2
View File
@@ -110,7 +110,7 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
cv::cvtColor(frame->img_pyr_[img_pub_level_], img_rgb, CV_GRAY2RGB);
// during initialization, draw lines
if(slam.stage() == FrameHandlerBase::SECOND_FRAME)
if(slam.stage() == FrameHandlerBase::STAGE_SECOND_FRAME)
{
const vector<cv::Point2f>& px_ref(slam.initFeatureTrackRefPx());
const vector<cv::Point2f>& px_cur(slam.initFeatureTrackCurPx());
@@ -148,7 +148,7 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
img_msg.encoding = sensor_msgs::image_encodings::BGR8;
pub_images_.publish(img_msg.toImageMsg());
}
else if(frame == NULL && pub_images_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::PAUSED)
else if(frame == NULL && pub_images_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_PAUSED)
{
// display image when slam is not running
cv_bridge::CvImage img_msg;
svo_ros/src/vo_node.cpp
+2 -2
View File
@@ -110,13 +110,13 @@ imgCb(const sensor_msgs::ImageConstPtr& msg)
vo_->addImage(img, msg->header.stamp.toSec());
visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec());
if(publish_markers_ && vo_->stage() != FrameHandlerBase::PAUSED)
if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED)
visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
if(publish_dense_input_)
visualizer_.exportToDense(vo_->lastFrame());
if(vo_->stage() == FrameHandlerMono::PAUSED)
if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED)
usleep(100000); // avoid busy loop when paused
}