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