refactoring Point, added point normal as member

svo/include/svo/point.h

@@ -36,24 +36,27 @@ class Point : boost::noncopyable
 {
 public:
 
-  enum PointType {DELETED, CANDIDATE, UNKNOWN_QUALITY, GOOD_QUALITY };
+  enum PointType {
-  enum PointOrigin { TRIANGULATION, DEPTH, VOGIATZIS, IDFILTER };
+    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<Feature*>              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.

svo/src/initialization.cpp

@@ -84,7 +84,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
     if(frame_ref_->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 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);

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))

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),
+  type_(TYPE_UNKNOWN),
-  origin_(origin),
   n_failed_reproj_(0),
   n_succeeded_reproj_(0),
   last_structure_optim_(0)

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)
+    if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
-      it->pt->type_ = Point::GOOD_QUALITY;
+      it->pt->type_ = Point::TYPE_GOOD;
 
     Feature* new_feature = new Feature(frame.get(), it->px, feature_level);
     frame->addFeature(new_feature);

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<float>(it->y, it->x)), Point::DEPTH));
+        Point* point(new Point(frame_->f2w(ftr->f*depthmap_.at<float>(it->y, it->x))));
         ftr->point = point;
         ftr->frame = frame_.get();
         frame_->addFeature(ftr);

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<float>(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);
       }

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<float>(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);