From 6a34b59f723639350d48a1a2b1272b8ec339a40c Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Mon, 12 May 2014 22:26:12 +0200 Subject: [PATCH] added uncertainty for patch normal --- svo/include/svo/point.h | 1 + svo/src/point.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/svo/include/svo/point.h b/svo/include/svo/point.h index 3af92d6..f7706be 100644 --- a/svo/include/svo/point.h +++ b/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 obs_; //!< References to keyframes which observe the point. size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames. diff --git a/svo/src/point.cpp b/svo/src/point.cpp index 48c084d..2e45e06 100644 --- a/svo/src/point.cpp +++ b/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(pow(20/(pos_-ftr->frame->pos()).norm(),2), 1.0, 1.0); normal_set_ = true; }