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

renamed state machine enum variables to comply with ros style guide

Browse Source
This commit is contained in:
Christian Forster
2014-03-27 11:25:06 +01:00
parent 12451cff17
commit 536cf3396d
6 changed files with 61 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/include/svo/frame_handler_base.h
+22 -8
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();
svo/src/frame_handler_base.cpp
+14 -13
View File
@@ -28,7 +28,8 @@
#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
@@ -37,13 +38,13 @@ vk::PerformanceMonitor* g_permon = NULL;
FrameHandlerBase::
FrameHandlerBase() :
stage_(PAUSED),
stage_(STAGE_PAUSED),
set_reset_(false),
set_start_(false),
acc_frame_timings_(10),
acc_num_obs_(10),
num_obs_last_(0),
tracking_quality_(BAD),
tracking_quality_(TRACKING_BAD),
depth_filter_(NULL)
{
#ifdef SVO_TRACE
@@ -96,10 +97,10 @@ 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);
@@ -123,7 +124,7 @@ finishFrameProcessingCommon(
// 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");
@@ -138,7 +139,7 @@ finishFrameProcessingCommon(
#endif
// reset if failure or requested
if(dropout == FAILURE || set_reset_)
if(dropout == RESULT_FAILURE || set_reset_)
resetAll();
return 0;
}
@@ -147,10 +148,10 @@ void FrameHandlerBase::
resetCommon()
{
map_.reset();
stage_ = PAUSED;
stage_ = STAGE_PAUSED;
set_reset_ = false;
set_start_ = false;
tracking_quality_ = BAD;
tracking_quality_ = TRACKING_BAD;
depth_filter_->reset();
num_obs_last_ = 0;
SVO_INFO_STREAM("RESET");
@@ -159,22 +160,22 @@ resetCommon()
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;
}
}
svo/src/frame_handler_mono.cpp
+19 -19
View File
@@ -58,12 +58,12 @@ addImage(const cv::Mat& img, const 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
@@ -81,7 +81,7 @@ setFirstFrame(const FramePtr& first_frame)
last_frame_ = first_frame;
last_frame_->setKeyframe();
map_.addKeyframe(last_frame_);
stage_ = DEFAULT_FRAME;
stage_ = STAGE_DEFAULT_FRAME;
}
FrameHandlerMono::UpdateResult FrameHandlerMono::
@@ -89,12 +89,12 @@ 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::
@@ -102,9 +102,9 @@ 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,10 +118,10 @@ 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::
@@ -150,7 +150,7 @@ processFrame()
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,7 +224,7 @@ processFrame()
new_frame_->setKeyframe();
map_.addKeyframe(new_frame_);
return IS_KEYFRAME;
return RESULT_IS_KEYFRAME;
}
void FrameHandlerMono::
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
}