added uncertainty for patch normal

svo/include/svo/point.h

@@ -47,6 +47,7 @@ public:
   int                         id_;                      //!< Unique ID of the point.
   Vector3d                    pos_;                     //!< 3d pos of the point in the world coordinate frame.
   Vector3d                    normal_;                  //!< Surface normal at point.
+  Matrix3d                    normal_information_;      //!< Inverse covariance matrix of normal estimation.
   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.

svo/src/point.cpp

@@ -90,6 +90,7 @@ void Point::initNormal()
   const Feature* ftr = obs_.back();
   assert(ftr->frame != NULL);
   normal_ = ftr->frame->T_f_w_.rotation_matrix().transpose()*(-ftr->f);
+  normal_information_ = DiagonalMatrix<double,3,3>(pow(20/(pos_-ftr->frame->pos()).norm(),2), 1.0, 1.0);
   normal_set_ = true;
 }