diff --git a/svo/include/svo/point.h b/svo/include/svo/point.h index 1fa26f0..933b8bc 100644 --- a/svo/include/svo/point.h +++ b/svo/include/svo/point.h @@ -36,24 +36,27 @@ class Point : boost::noncopyable { public: - enum PointType {DELETED, CANDIDATE, UNKNOWN_QUALITY, GOOD_QUALITY }; - enum PointOrigin { TRIANGULATION, DEPTH, VOGIATZIS, IDFILTER }; + enum PointType { + TYPE_DELETED, + TYPE_CANDIDATE, + TYPE_UNKNOWN, + TYPE_GOOD }; static int point_counter_; //!< Counts the number of created points. Used to set the unique id. int id_; //!< Unique ID of the point. Vector3d pos_; //!< 3d pos of the point in the world coordinate frame. + Vector3d normal_; //!< Normal of the point list obs_; //!< References to keyframes which observe the point. - size_t n_obs_; //!< Number of obervations, not only keyframes. + size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames. g2oPoint* v_pt_; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment. int last_published_ts_; //!< Timestamp of last publishing. int last_projected_kf_id_; //!< Flag for the reprojection: don't reproject a pt twice. PointType type_; //!< Quality of the point. - PointOrigin origin_; //!< What is the source of the point, i.e. DEPTH if a kinect is used. int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point. int n_succeeded_reproj_; //!< Number of succeeded reprojections. Used to assess the quality of the point. - int last_structure_optim_; + int last_structure_optim_; //!< Timestamp of last point optimization - Point(Vector3d pos, PointOrigin origin); + Point(Vector3d pos); ~Point(); /// Add a reference to a frame. diff --git a/svo/src/initialization.cpp b/svo/src/initialization.cpp index 1087e61..2aa3931 100644 --- a/svo/src/initialization.cpp +++ b/svo/src/initialization.cpp @@ -84,7 +84,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur) if(frame_ref_->cam_->isInFrame(px_cur.cast(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast(), 10) && xyz_in_cur_[*it].z() > 0) { Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale); - Point* new_point = new Point(pos, Point::TRIANGULATION); + Point* new_point = new Point(pos); Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0)); frame_cur->addFeature(ftr_cur); diff --git a/svo/src/map.cpp b/svo/src/map.cpp index 808917c..1decc22 100644 --- a/svo/src/map.cpp +++ b/svo/src/map.cpp @@ -94,7 +94,7 @@ void Map::safeDeletePoint(Point* pt) void Map::deletePoint(Point* pt) { - pt->type_ = Point::DELETED; + pt->type_ = Point::TYPE_DELETED; trash_points_.push_back(pt); } @@ -196,9 +196,9 @@ MapPointCandidates::~MapPointCandidates() void MapPointCandidates::newCandidatePoint(const Seed& seed, const Vector3d& xyz_world) { // initalize a new point - Point* point = new Point(xyz_world, Point::VOGIATZIS); + Point* point = new Point(xyz_world); point->addFrameRef(seed.ftr); - point->type_ = Point::CANDIDATE; + point->type_ = Point::TYPE_CANDIDATE; seed.ftr->point = point; // add candidate to list @@ -215,7 +215,7 @@ void MapPointCandidates::addCandidatePointToFrame(FramePtr frame) if(it->first->obs_.front()->frame == frame.get()) { // insert feature in the frame - it->first->type_ = Point::UNKNOWN_QUALITY; + it->first->type_ = Point::TYPE_UNKNOWN; it->first->n_failed_reproj_ = 0; it->second->frame->addFeature(it->second); it = candidates_.erase(it); @@ -271,7 +271,7 @@ void MapPointCandidates::deleteCandidate(PointCandidate& c) // camera-rig: another frame might still be pointing to the candidate point // therefore, we can't delete it right now. delete c.second; c.second=NULL; - c.first->type_ = Point::DELETED; + c.first->type_ = Point::TYPE_DELETED; trash_points_.push_back(c.first); } @@ -298,7 +298,7 @@ void frameValidation(Frame* frame, int id) if((*it)->point==NULL) continue; - if((*it)->point->type_ == Point::DELETED) + if((*it)->point->type_ == Point::TYPE_DELETED) printf("ERROR DataValidation %i: Referenced point was deleted.\n", id); if(!(*it)->point->findFrameRef(frame)) diff --git a/svo/src/point.cpp b/svo/src/point.cpp index 40aced9..75f14a0 100644 --- a/svo/src/point.cpp +++ b/svo/src/point.cpp @@ -24,15 +24,15 @@ namespace svo { int Point::point_counter_ = 0; -Point::Point(Vector3d pos, PointOrigin origin) : +Point::Point(Vector3d pos) : id_(point_counter_++), pos_(pos), + normal_(Vector3d::Zero()), n_obs_(0), v_pt_(NULL), last_published_ts_(0), last_projected_kf_id_(-1), - type_(UNKNOWN_QUALITY), - origin_(origin), + type_(TYPE_UNKNOWN), n_failed_reproj_(0), n_succeeded_reproj_(0), last_structure_optim_(0) diff --git a/svo/src/reprojection.cpp b/svo/src/reprojection.cpp index 26de82c..61de512 100644 --- a/svo/src/reprojection.cpp +++ b/svo/src/reprojection.cpp @@ -158,7 +158,7 @@ bool reprojectCell( { ++n_trials; - if(it->pt->type_ == Point::DELETED) + if(it->pt->type_ == Point::TYPE_DELETED) { it = cell.erase(it); continue; @@ -169,16 +169,16 @@ bool reprojectCell( if(!found_match) { it->pt->n_failed_reproj_++; - if(it->pt->type_ == Point::UNKNOWN_QUALITY && it->pt->n_failed_reproj_ > 15) + if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15) map.safeDeletePoint(it->pt); - if(it->pt->type_ == Point::CANDIDATE && it->pt->n_failed_reproj_ > 30) + if(it->pt->type_ == Point::TYPE_CANDIDATE && it->pt->n_failed_reproj_ > 30) candidate_points.deleteCandidatePoint(it->pt); it = cell.erase(it); continue; } it->pt->n_succeeded_reproj_++; - if(it->pt->type_ == Point::UNKNOWN_QUALITY && it->pt->n_succeeded_reproj_ > 10) - it->pt->type_ = Point::GOOD_QUALITY; + if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10) + it->pt->type_ = Point::TYPE_GOOD; Feature* new_feature = new Feature(frame.get(), it->px, feature_level); frame->addFeature(new_feature); diff --git a/svo/test/test_pose_optimizer.cpp b/svo/test/test_pose_optimizer.cpp index 0c9a63f..16f3dfc 100644 --- a/svo/test/test_pose_optimizer.cpp +++ b/svo/test/test_pose_optimizer.cpp @@ -69,7 +69,7 @@ class PoseOptimizerTest { if(it->score > Config::triangMinCornerScore()) { Feature* ftr(new Feature(frame_.get(), Vector2d(it->x, it->y), it->level)); - Point* point(new Point(frame_->f2w(ftr->f*depthmap_.at(it->y, it->x)), Point::DEPTH)); + Point* point(new Point(frame_->f2w(ftr->f*depthmap_.at(it->y, it->x)))); ftr->point = point; ftr->frame = frame_.get(); frame_->addFeature(ftr); diff --git a/svo/test/test_sparse_img_align.cpp b/svo/test/test_sparse_img_align.cpp index 51950a8..b4f5f3b 100644 --- a/svo/test/test_sparse_img_align.cpp +++ b/svo/test/test_sparse_img_align.cpp @@ -104,7 +104,7 @@ void SparseImgAlignTest::testSequence( svo::Feature* ftr = new svo::Feature(frame_ref_.get(), Eigen::Vector2d(corner->x, corner->y), corner->level); Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at(corner->y, corner->x); Eigen::Vector3d pt_pos_w = frame_ref_->T_f_w_.inverse()*pt_pos_cur; - svo::Point* pt = new svo::Point(pt_pos_w, svo::Point::DEPTH); + svo::Point* pt = new svo::Point(pt_pos_w); ftr->point = pt; frame_ref_->addFeature(ftr); } diff --git a/svo_ros/src/benchmark_node.cpp b/svo_ros/src/benchmark_node.cpp index 561f131..13ee94b 100644 --- a/svo_ros/src/benchmark_node.cpp +++ b/svo_ros/src/benchmark_node.cpp @@ -202,7 +202,7 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir) svo::Feature* ftr = new svo::Feature(frame_ref.get(), Eigen::Vector2d(corner->x, corner->y), corner->level); Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at(corner->y, corner->x); Eigen::Vector3d pt_pos_world = frame_ref->T_f_w_.inverse()*pt_pos_cur; - svo::Point* point = new svo::Point(pt_pos_world, svo::Point::DEPTH); + svo::Point* point = new svo::Point(pt_pos_world); ftr->point = point; ftr->point->addFrameRef(ftr); frame_ref->addFeature(ftr);