include/svo/depth_filter.h

Go to the documentation of this file.

// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// 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 SVO_DEPTH_FILTER_H_
#define SVO_DEPTH_FILTER_H_

#include <queue>
#include <boost/thread.hpp>
#include <boost/function.hpp>
#include <vikit/performance_monitor.h>
#include <svo/global.h>
#include <svo/feature_detection.h>
#include <svo/matcher.h>

namespace svo {

class Frame;
class Feature;
class Point;

struct Seed
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  static int batch_counter;
  static int seed_counter;
  int batch_id;                
  int id;                      
  Feature* ftr;                
  float a;                     
  float b;                     
  float mu;                    
  float z_range;               
  float sigma2;                
  Matrix2d patch_cov;          
  Seed(Feature* ftr, float depth_mean, float depth_min);
};

class DepthFilter
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef boost::unique_lock<boost::mutex> lock_t;
  typedef boost::function<void ( Point*, double )> callback_t;

  struct Options
  {
    bool check_ftr_angle;                       
    bool epi_search_1d;                         
    bool verbose;                               
    bool use_photometric_disparity_error;       
    int max_n_kfs;                              
    double sigma_i_sq;                          
    double seed_convergence_sigma2_thresh;      
    Options()
    : check_ftr_angle(false),
      epi_search_1d(false),
      verbose(false),
      use_photometric_disparity_error(false),
      max_n_kfs(3),
      sigma_i_sq(5e-4),
      seed_convergence_sigma2_thresh(200.0)
    {}
  } options_;

  DepthFilter(
      feature_detection::DetectorPtr feature_detector,
      callback_t seed_converged_cb);

  virtual ~DepthFilter();

  void startThread();

  void stopThread();

  void addFrame(FramePtr frame);

  void addKeyframe(FramePtr frame, double depth_mean, double depth_min);

  void removeKeyframe(FramePtr frame);

  void reset();

  void getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds);

  std::list<Seed, aligned_allocator<Seed> >& getSeeds() { return seeds_; }

  static void updateSeed(
      const float x,
      const float tau2,
      Seed* seed);

  static double computeTau(
      const SE3& T_ref_cur,
      const Vector3d& f,
      const double z,
      const double px_error_angle);

protected:
  feature_detection::DetectorPtr feature_detector_;
  callback_t seed_converged_cb_;
  std::list<Seed, aligned_allocator<Seed> > seeds_;
  boost::mutex seeds_mut_;
  bool seeds_updating_halt_;            
  boost::thread* thread_;
  std::queue<FramePtr> frame_queue_;
  boost::mutex frame_queue_mut_;
  boost::condition_variable frame_queue_cond_;
  FramePtr new_keyframe_;               
  bool new_keyframe_set_;               
  double new_keyframe_min_depth_;       
  double new_keyframe_mean_depth_;      
  vk::PerformanceMonitor permon_;       
  Matcher matcher_;

  void initializeSeeds(FramePtr frame);

  virtual void updateSeeds(FramePtr frame);

  void clearFrameQueue();

  void updateSeedsLoop();
};

} // namespace svo

#endif // SVO_DEPTH_FILTER_H_