ThreadedKFVio.hpp

Go to the documentation of this file.

/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 * 
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Aug 21, 2014
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *    Modified: Andreas Forster (an.forster@gmail.com)
 *********************************************************************************/

#ifndef INCLUDE_OKVIS_THREADEDKFVIO_HPP_
#define INCLUDE_OKVIS_THREADEDKFVIO_HPP_

#include <thread>
#include <mutex>
#include <condition_variable>
#include <atomic>

#include <okvis/cameras/NCameraSystem.hpp>
#include <okvis/Measurements.hpp>
#include <okvis/Frontend.hpp>
#include <okvis/MultiFrame.hpp>
#include <okvis/Parameters.hpp>
#include <okvis/assert_macros.hpp>

#include <okvis/ImuFrameSynchronizer.hpp>
#include <okvis/FrameSynchronizer.hpp>
#include <okvis/VioVisualizer.hpp>
#include <okvis/timing/Timer.hpp>
#include <okvis/threadsafe/ThreadsafeQueue.hpp>

#ifdef USE_MOCK
#include <../test/MockVioFrontendInterface.hpp>
#include <../test/MockVioBackendInterface.hpp>
#else
#include <okvis/Estimator.hpp>
#include <okvis/VioFrontendInterface.hpp>
#endif

namespace okvis {

class ThreadedKFVio : public VioInterface {
 public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error)

#ifdef DEACTIVATE_TIMERS
  typedef okvis::timing::DummyTimer TimerSwitchable;
#else
  typedef okvis::timing::Timer TimerSwitchable;
#endif

#ifdef USE_MOCK

  ThreadedKFVio(okvis::VioParameters& parameters, okvis::MockVioBackendInterface& estimator,
      okvis::MockVioFrontendInterface& frontend);

#else

  ThreadedKFVio(okvis::VioParameters& parameters);
#endif

  virtual ~ThreadedKFVio();


  virtual bool addImage(const okvis::Time & stamp, size_t cameraIndex,
                        const cv::Mat & image,
                        const std::vector<cv::KeyPoint> * keypoints = 0,
                        bool* asKeyframe = 0);

  virtual bool addKeypoints(const okvis::Time & stamp, size_t cameraIndex,
                            const std::vector<cv::KeyPoint> & keypoints,
                            const std::vector<uint64_t> & landmarkIds,
                            const cv::Mat& descriptors = cv::Mat(),
                            bool* asKeyframe = 0);

  virtual bool addImuMeasurement(const okvis::Time & stamp,
                                 const Eigen::Vector3d & alpha,
                                 const Eigen::Vector3d & omega);

  virtual void addPositionMeasurement(
      const okvis::Time & stamp, const Eigen::Vector3d & position,
      const Eigen::Vector3d & positionOffset,
      const Eigen::Matrix3d & positionCovariance);

  virtual void addGpsMeasurement(const okvis::Time & stamp,
                                 double lat_wgs84_deg, double lon_wgs84_deg,
                                 double alt_wgs84_deg,
                                 const Eigen::Vector3d & positionOffset,
                                 const Eigen::Matrix3d & positionCovarianceENU);

  virtual void addMagnetometerMeasurement(
      const okvis::Time & stamp, const Eigen::Vector3d & fluxDensityMeas,
      double stdev);

  virtual void addBarometerMeasurement(const okvis::Time & stamp,
                                       double staticPressure, double stdev);

  virtual void addDifferentialPressureMeasurement(const okvis::Time & stamp,
                                                  double differentialPressure,
                                                  double stdev);


  virtual void setBlocking(bool blocking);


  void display();

 private:
  virtual void startThreads();
  void init();

 private:

  void frameConsumerLoop(size_t cameraIndex);
  void matchingLoop();
  void imuConsumerLoop();
  void positionConsumerLoop();
  void gpsConsumerLoop();
  void magnetometerConsumerLoop();
  void differentialConsumerLoop();

  void visualizationLoop();
  void optimizationLoop();
  void publisherLoop();

  okvis::ImuMeasurementDeque getImuMeasurments(okvis::Time& start,
                                               okvis::Time& end);

  int deleteImuMeasurements(const okvis::Time& eraseUntil);

 private:

  struct OptimizationResults {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    okvis::Time stamp;                          
    okvis::kinematics::Transformation T_WS;     
    okvis::SpeedAndBias speedAndBiases;         
    Eigen::Matrix<double, 3, 1> omega_S;        
    std::vector<okvis::kinematics::Transformation,
        Eigen::aligned_allocator<okvis::kinematics::Transformation> > vector_of_T_SCi;
    okvis::MapPointVector landmarksVector;      
    okvis::MapPointVector transferredLandmarks; 
    bool onlyPublishLandmarks;                  
  };


  okvis::SpeedAndBias speedAndBiases_propagated_;     
  okvis::ImuParameters imu_params_;
  okvis::kinematics::Transformation T_WS_propagated_; 
  std::shared_ptr<okvis::MapPointVector> map_;        

  // lock lastState_mutex_ when accessing these
  okvis::kinematics::Transformation lastOptimized_T_WS_;
  okvis::SpeedAndBias lastOptimizedSpeedAndBiases_;
  okvis::Time lastOptimizedStateTimestamp_;
  std::atomic_bool repropagationNeeded_;


  ImuFrameSynchronizer imuFrameSynchronizer_;  
  okvis::FrameSynchronizer frameSynchronizer_;

  okvis::Time lastAddedStateTimestamp_; 
  okvis::Time lastAddedImageTimestamp_; 



  std::vector<std::shared_ptr<
      okvis::threadsafe::ThreadSafeQueue<std::shared_ptr<okvis::CameraMeasurement> > > > cameraMeasurementsReceived_;
  okvis::threadsafe::ThreadSafeQueue<okvis::ImuMeasurement> imuMeasurementsReceived_;

  okvis::threadsafe::ThreadSafeQueue<okvis::PositionMeasurement> positionMeasurementsReceived_;


  okvis::threadsafe::ThreadSafeQueue<std::shared_ptr<okvis::MultiFrame> > keypointMeasurements_;
  okvis::threadsafe::ThreadSafeQueue<std::shared_ptr<okvis::MultiFrame>> matchedFrames_;
  okvis::ImuMeasurementDeque imuMeasurements_;
  okvis::PositionMeasurementDeque positionMeasurements_;
  okvis::threadsafe::ThreadSafeQueue<OptimizationResults> optimizationResults_;
  okvis::threadsafe::ThreadSafeQueue<VioVisualizer::VisualizationData::Ptr> visualizationData_;
  okvis::threadsafe::ThreadSafeQueue<std::vector<cv::Mat>> displayImages_;


  std::mutex imuMeasurements_mutex_;      
  std::mutex positionMeasurements_mutex_;      
  std::mutex frameSynchronizer_mutex_;    
  std::mutex estimator_mutex_;            
  std::condition_variable optimizationNotification_;
  std::atomic_bool optimizationDone_;
  std::mutex lastState_mutex_;            


  std::vector<std::thread> frameConsumerThreads_;
  std::vector<std::thread> keypointConsumerThreads_;  
  std::vector<std::thread> matchesConsumerThreads_;   
  std::thread imuConsumerThread_;           
  std::thread positionConsumerThread_;      
  std::thread gpsConsumerThread_;           
  std::thread magnetometerConsumerThread_;  
  std::thread differentialConsumerThread_;  


  std::thread visualizationThread_; 
  std::thread optimizationThread_;  
  std::thread publisherThread_;     


#ifdef USE_MOCK
  okvis::MockVioBackendInterface& estimator_;
  okvis::MockVioFrontendInterface& frontend_;
#else
  okvis::Estimator estimator_;    
  okvis::Frontend frontend_;      
#endif


  size_t numCameras_;     
  size_t numCameraPairs_; 

  okvis::VioParameters parameters_; 

  const size_t maxImuInputQueueSize_;

  const size_t maxPositionInputQueueSize_ = 10;
  
};

}  // namespace okvis

#endif /* INCLUDE_OKVIS_THREADEDKFVIO_HPP_ */