Merge pull request #2 from uzh-rpg/dev

blabla

svo/include/svo/config.h

@@ -14,16 +14,16 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#ifndef NSLAM_CONFIG_H_
-#define NSLAM_CONFIG_H_
+#ifndef SVO_CONFIG_H_
+#define SVO_CONFIG_H_
 
 #include <string>
 #include <stdint.h>
 #include <stdio.h>
 
 namespace svo {
 
-using namespace std;
+using std::string;
 
 /// Global configuration file of SVO.
 /// Implements the Singleton design pattern to allow global access and to ensure
@@ -154,6 +154,6 @@ class Config
   int quality_max_drop_fts;
 };
 
-} // namespace nslam
+} // namespace svo
 
-#endif // NSLAM_CONFIG_H_
+#endif // SVO_CONFIG_H_

svo/include/svo/depth_filter.h

@@ -46,7 +46,7 @@ struct Seed
   float z_range;               //!< Max range of the possible depth.
   float sigma2;                //!< Variance of normal distribution.
   Matrix2d patch_cov;          //!< Patch covariance in reference image.
-  Seed(Feature* ftr, float angle, float depth_mean, float depth_min);
+  Seed(Feature* ftr, float depth_mean, float depth_min);
 };
 
 /// Depth filter implements the Bayesian Update proposed in:
@@ -64,7 +64,8 @@ class DepthFilter
   typedef boost::function<void ( const Seed&, const Vector3d& )> callback_t;
 
   /// Depth-filter config parameters
-  struct Options {
+  struct Options
+  {
     bool check_ftr_angle;                       //!< gradient features are only updated if the epipolar line is orthogonal to the gradient.
     bool epi_search_1d;                         //!< restrict Gauss Newton in the epipolar search to the epipolar line.
     bool verbose;                               //!< display output.
@@ -83,8 +84,11 @@ class DepthFilter
     {}
   } options_;
 
-  DepthFilter(feature_detection::DetectorPtr feature_detector, callback_t seed_converged_cb);
-  ~DepthFilter();
+  DepthFilter(
+      feature_detection::DetectorPtr feature_detector,
+      callback_t seed_converged_cb);
+
+  virtual ~DepthFilter();
 
   /// Start this thread when seed updating should be in a parallel thread.
   void startThread();
@@ -129,7 +133,7 @@ class DepthFilter
       const double z,
       const double px_error_angle);
 
-private:
+protected:
   feature_detection::DetectorPtr feature_detector_;
   callback_t seed_converged_cb_;
   std::list<Seed, aligned_allocator<Seed> > seeds_;
@@ -149,7 +153,7 @@ class DepthFilter
   void initializeSeeds(FramePtr frame);
 
   /// Update all seeds with a new measurement frame.
-  void updateSeeds(FramePtr frame);
+  virtual void updateSeeds(FramePtr frame);
 
   /// When a new keyframe arrives, the frame queue should be cleared.
   void clearFrameQueue();

svo/include/svo/feature.h

@@ -26,34 +26,47 @@ struct Feature
 {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  Frame*   frame;       //!< Pointer to frame in which the feature was detected.
+  enum FeatureType {
+    CORNER,
+    EDGELET
+  };
+
+  FeatureType type;     //!< Type can be corner or edgelet.
+  Frame* frame;         //!< Pointer to frame in which the feature was detected.
   Vector2d px;          //!< Coordinates in pixels on pyramid level 0.
   Vector3d f;           //!< Unit-bearing vector of the feature.
-  int      level;       //!< Image pyramid level where feature was extracted.
-  Point*   point;       //!< Pointer to 3D point which corresponds to the feature.
-
-  Feature(Frame* _frame, const Vector2d& _px, int _level)
-   : frame(_frame),
-     px(_px),
-     f(frame->cam_->cam2world(px)),
-     level(_level),
-     point(NULL)
+  int level;            //!< Image pyramid level where feature was extracted.
+  Point* point;         //!< Pointer to 3D point which corresponds to the feature.
+  Vector2d grad;        //!< Dominant gradient direction for edglets, normalized.
+
+  Feature(Frame* _frame, const Vector2d& _px, int _level) :
+    type(CORNER),
+    frame(_frame),
+    px(_px),
+    f(frame->cam_->cam2world(px)),
+    level(_level),
+    point(NULL),
+    grad(0.0,0.0)
   {}
 
-  Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level)
-  : frame(_frame),
+  Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) :
+    type(CORNER),
+    frame(_frame),
     px(_px),
     f(_f),
     level(_level),
-    point(NULL)
+    point(NULL),
+    grad(0.0,0.0)
   {}
 
-  Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level)
-    : frame(_frame),
-      px(_px),
-      f(_f),
-      level(_level),
-      point(_point)
+  Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) :
+    type(CORNER),
+    frame(_frame),
+    px(_px),
+    f(_f),
+    level(_level),
+    point(_point),
+    grad(0.0,0.0)
   {}
 };
 

svo/include/svo/feature_detection.h

@@ -26,7 +26,6 @@ namespace svo {
 namespace feature_detection {
 
 /// Temporary container used for corner detection. Features are initialized from these.
-// TODO: why not use feature struct?
 struct Corner
 {
   int x;        //!< x-coordinate of corner in the image.
@@ -44,30 +43,62 @@ typedef vector<Corner> Corners;
 class AbstractDetector
 {
 public:
+  AbstractDetector(
+      const int img_width,
+      const int img_height,
+      const int cell_size,
+      const int n_pyr_levels);
+
   virtual ~AbstractDetector() {};
+
   virtual void detect(
+      Frame* frame,
       const ImgPyr& img_pyr,
-      const Features& fts,
-      const int cell_size,
-      const int n_levels,
       const double detection_threshold,
-      Corners* corners) const = 0;
+      Features& fts) = 0;
+
+  /// Flag the grid cell as occupied
+  void setGridOccpuancy(const Vector2d& px);
+
+  /// Set grid cells of existing features as occupied
+  void setExistingFeatures(const Features& fts);
+
+protected:
+
+  static const int border_ = 8; //!< no feature should be within 8px of border.
+  const int cell_size_;
+  const int n_pyr_levels_;
+  const int grid_n_cols_;
+  const int grid_n_rows_;
+  vector<bool> grid_occupancy_;
+
+  void resetGrid();
+
+  inline int getCellIndex(int x, int y, int level)
+  {
+    const int scale = (1<<level);
+    return (scale*y)/cell_size_*grid_n_cols_ + (scale*x)/cell_size_;
+  }
 };
 typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
 
 /// FAST detector by Edward Rosten.
 class FastDetector : public AbstractDetector
 {
 public:
-  FastDetector() {}
+  FastDetector(
+      const int img_width,
+      const int img_height,
+      const int cell_size,
+      const int n_pyr_levels);
+
   virtual ~FastDetector() {}
+
   virtual void detect(
-        const ImgPyr& img_pyr,
-        const Features& fts,
-        const int cell_size,
-        const int n_levels,
-        const double detection_threshold,
-        Corners* corners) const;
+      Frame* frame,
+      const ImgPyr& img_pyr,
+      const double detection_threshold,
+      Features& fts);
 };
 
 } // namespace feature_detection

svo/include/svo/frame.h

@@ -79,7 +79,7 @@ class Frame : boost::noncopyable
   void removeKeyPoint(Feature* ftr);
 
   /// Return number of point observations.
-  size_t nObs() const;
+  inline size_t nObs() const { return fts_.size(); }
 
   /// Check if a point in (w)orld coordinate frame is visible in the image.
   bool isVisible(const Vector3d& xyz_w) const;

svo/include/svo/frame_handler_base.h

@@ -63,9 +63,6 @@ class FrameHandlerBase : boost::noncopyable
 
   virtual ~FrameHandlerBase();
 
-  /// Set initial pose of the camera.
-  void setInitialPose(const SE3& T_frame_from_world) { T_f_w_init_ = T_frame_from_world; }
-
   /// Get the current map.
   const Map& map() const { return map_; }
 
@@ -92,7 +89,6 @@ class FrameHandlerBase : boost::noncopyable
   bool set_reset_;              //!< Flag that the user can set. Will reset the system before the next iteration.
   bool set_start_;              //!< Flag the user can set to start the system when the next image is received.
   Map map_;                     //!< Map of keyframes created by the slam system.
-  SE3 T_f_w_init_;              //!< Initial pose of the camera.
   vk::Timer timer_;             //!< Stopwatch to measure time to process frame.
   vk::RingBuffer<double> acc_frame_timings_;    //!< Total processing time of the last 10 frames, used to give some user feedback on the performance.
   vk::RingBuffer<size_t> acc_num_obs_;          //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance.

svo/include/svo/point.h

@@ -56,7 +56,8 @@ class Point : boost::noncopyable
   int                         n_succeeded_reproj_;      //!< Number of succeeded reprojections. Used to assess the quality of the point.
   int                         last_structure_optim_;    //!< Timestamp of last point optimization
 
-  Point(Vector3d pos);
+  Point(const Vector3d& pos);
+  Point(const Vector3d& pos, Feature* ftr);
   ~Point();
 
   /// Add a reference to a frame.

svo/include/svo/sparse_img_align.h

@@ -51,6 +51,10 @@ class SparseImgAlign : public vk::NLLSSolver<6, SE3>
       FramePtr ref_frame,
       FramePtr cur_frame);
 
+  /// Return fisher information matrix, i.e. the Hessian of the log-likelihood
+  /// at the converged state.
+  Matrix<double, 6, 6> getFisherInformation();
+
 protected:
   FramePtr ref_frame_;            //!< reference frame, has depth for gradient pixels.
   FramePtr cur_frame_;            //!< only the image is known!

svo/src/depth_filter.cpp

@@ -34,7 +34,7 @@ namespace svo {
 int Seed::batch_counter = 0;
 int Seed::seed_counter = 0;
 
-Seed::Seed(Feature* ftr, float angle, float depth_mean, float depth_min) :
+Seed::Seed(Feature* ftr, float depth_mean, float depth_min) :
     batch_id(batch_counter),
     id(seed_counter++),
     ftr(ftr),
@@ -113,32 +113,20 @@ void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_mi
 
 void DepthFilter::initializeSeeds(FramePtr frame)
 {
-  feature_detection::Corners corners;
-  feature_detector_->detect(frame->img_pyr_, frame->fts_, svo::Config::gridSize(),
-                            svo::Config::nPyrLevels(), Config::triangMinCornerScore(),
-                            &corners);
+  Features new_features;
+  feature_detector_->setExistingFeatures(frame->fts_);
+  feature_detector_->detect(frame.get(), frame->img_pyr_,
+                            Config::triangMinCornerScore(), new_features);
 
+  // initialize a seed for every new feature
   seeds_updating_halt_ = true;
   lock_t lock(seeds_mut_); // by locking the updateSeeds function stops
-  size_t new_seeds = 0;
   ++Seed::batch_counter;
+  std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
+    seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
+  });
 
-  // now for all maximum corners, initialize a new seed
-  for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it)
-  {
-    // we must check again if the score is above the threshold because the Corners
-    // vector is initialized with dummy corners
-    if(it->score > Config::triangMinCornerScore())
-    {
-      Feature* new_feature(new Feature(frame.get(), Vector2d(it->x, it->y), it->level));
-      seeds_.push_back(Seed(new_feature, it->angle, new_keyframe_mean_depth_, new_keyframe_min_depth_));
-      ++new_seeds;
-    }
-  }
-
-  if(options_.verbose)
-    SVO_INFO_STREAM("DepthFilter: Initialized "<<new_seeds<<" new seeds");
-
+  SVO_DEBUG_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");
   seeds_updating_halt_ = false;
 }
 
@@ -174,8 +162,7 @@ void DepthFilter::reset()
     frame_queue_.pop();
   seeds_updating_halt_ = false;
 
-  if(options_.verbose)
-    SVO_INFO_STREAM("DepthFilter: RESET.");
+  SVO_DEBUG_STREAM("DepthFilter: RESET.");
 }
 
 void DepthFilter::updateSeedsLoop()
@@ -234,11 +221,8 @@ void DepthFilter::updateSeeds(FramePtr frame)
       ++it; // behind the camera
       continue;
     }
-    const Vector2d px(it->ftr->frame->f2c(xyz_f));
-    if(!it->ftr->frame->cam_->isInFrame(px.cast<int>())) {
-      ++it;
-      if(options_.verbose)
-    	SVO_DEBUG_STREAM("not visible");
+    if(!it->ftr->frame->cam_->isInFrame(it->ftr->frame->f2c(xyz_f).cast<int>())) {
+      ++it; // point does not project in image
       continue;
     }
 
@@ -250,25 +234,12 @@ void DepthFilter::updateSeeds(FramePtr frame)
         *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max,
         options_.epi_search_1d, z, h_inv))
     {
-      if(options_.verbose)
-        SVO_INFO_STREAM("no match found");
       it->b++; // increase outlier probability when no match was found
       ++it;
       ++n_failed_matches;
       continue;
     }
 
-    if(options_.use_photometric_disparity_error && h_inv > 0)
-    {
-      px_noise = fmax(2.0*options_.sigma_i_sq*h_inv, 1.0);
-      px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz)
-      if(options_.verbose)
-    	SVO_DEBUG_STREAM("Photometric disparity error "<<px_noise<<" px");
-    }
-
-    if(options_.verbose)
-      SVO_DEBUG_STREAM("updated seed");
-
     // compute tau
     double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle);
     double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
@@ -284,8 +255,6 @@ void DepthFilter::updateSeeds(FramePtr frame)
       {
         Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));
         seed_converged_cb_(*it, xyz_world);
-        if(options_.verbose)
-          SVO_DEBUG_STREAM("seed converged");
       }
       it = seeds_.erase(it);
     }
@@ -299,8 +268,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
   }
 }
 
-void DepthFilter::
-clearFrameQueue()
+void DepthFilter::clearFrameQueue()
 {
   while(!frame_queue_.empty())
     frame_queue_.pop();