added flag if patch normal is set

svo/include/svo/point.h

@@ -31,7 +31,7 @@ class Feature;
 
 typedef Matrix<double, 2, 3> Matrix23d;
 
-/// A 3D point that was triangulated from at least two corresponding image features.
+/// A 3D point on the surface of the scene.
 class Point : boost::noncopyable
 {
 public:
@@ -40,12 +40,14 @@ public:
     TYPE_DELETED,
     TYPE_CANDIDATE,
     TYPE_UNKNOWN,
-    TYPE_GOOD };
+    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.
-  Matrix3d                    R_w_p_;                   //!< Orientation of surface patch. Z-axis is the normal. Rotation from (p)oint frame to (w)orld frame.
+  Vector3d                    normal_;                  //!< Surface normal at point.
+  bool                        normal_set_;              //!< Flag whether the surface normal was estimated or not.
   list<Feature*>              obs_;                     //!< References to keyframes which observe the point.
   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.
@@ -66,6 +68,9 @@ public:
   /// Remove reference to a frame.
   bool deleteFrameRef(Frame* frame);
 
+  /// Initialize point normal. The inital estimate will point towards the frame.
+  void initNormal();
+
   /// Check whether mappoint has reference to a frame.
   Feature* findFrameRef(Frame* frame);
 

svo/src/point.cpp

@@ -27,6 +27,7 @@ int Point::point_counter_ = 0;
 Point::Point(const Vector3d& pos) :
   id_(point_counter_++),
   pos_(pos),
+  normal_set_(false),
   n_obs_(0),
   v_pt_(NULL),
   last_published_ts_(0),
@@ -40,6 +41,7 @@ Point::Point(const Vector3d& pos) :
 Point::Point(const Vector3d& pos, Feature* ftr) :
   id_(point_counter_++),
   pos_(pos),
+  normal_set_(false),
   n_obs_(1),
   v_pt_(NULL),
   last_published_ts_(0),
@@ -82,6 +84,15 @@ bool Point::deleteFrameRef(Frame* frame)
   return false;
 }
 
+void Point::initNormal()
+{
+  assert(!obs_.empty());
+  const Feature* ftr = obs_.back();
+  assert(ftr->frame != NULL);
+  normal_ = ftr->frame->T_f_w_.rotation_matrix().transpose()*(-ftr->f);
+  normal_set_ = true;
+}
+
 bool Point::getCloseViewObs(const Vector3d& framepos, Feature*& ftr) const
 {
   // TODO: get frame with same point of view AND same pyramid level!