VioInterface.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: Jun 11, 2013
 *      Author: Paul Furgale
 *    Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *    Modified: Andreas Forster (an.forster@gmail.com)
 *********************************************************************************/

#ifndef INCLUDE_OKVIS_VIOINTERFACE_HPP_
#define INCLUDE_OKVIS_VIOINTERFACE_HPP_

#include <cstdint>
#include <memory>
#include <functional>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#pragma GCC diagnostic pop
#include <okvis/assert_macros.hpp>
#include <okvis/Time.hpp>
#include <okvis/FrameTypedefs.hpp>
#include <okvis/kinematics/Transformation.hpp>

namespace okvis {

class VioInterface {
 public:
  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)

  typedef std::function<
  void(const okvis::Time &, const okvis::kinematics::Transformation &)> StateCallback;
  typedef std::function<
      void(const okvis::Time &, const okvis::kinematics::Transformation &,
           const Eigen::Matrix<double, 9, 1> &,
           const Eigen::Matrix<double, 3, 1> &)> FullStateCallback;
  typedef std::function<
      void(
          const okvis::Time &,
          const okvis::kinematics::Transformation &,
          const Eigen::Matrix<double, 9, 1> &,
          const Eigen::Matrix<double, 3, 1> &,
          const std::vector<okvis::kinematics::Transformation,
              Eigen::aligned_allocator<okvis::kinematics::Transformation> >&)> FullStateCallbackWithExtrinsics;
  typedef Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic> EigenImage;
  typedef std::function<
      void(const okvis::Time &, const okvis::MapPointVector &,
           const okvis::MapPointVector &)> LandmarksCallback;

  VioInterface();
  virtual ~VioInterface();


  bool setImuCsvFile(std::fstream& csvFile);
  bool setImuCsvFile(const std::string& csvFileName);

  bool setTracksCsvFile(size_t cameraId, std::fstream& csvFile);
  bool setTracksCsvFile(size_t cameraId, const std::string& csvFileName);

  bool setPosCsvFile(std::fstream& csvFile);
  bool setPosCsvFile(const std::string& csvFileName);

  bool setMagCsvFile(std::fstream& csvFile);
  bool setMagCsvFile(const std::string& csvFileName);


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

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

  /*
  */
  virtual void addPositionMeasurement(
      const okvis::Time & /*stamp*/, const Eigen::Vector3d & /*position*/,
      const Eigen::Vector3d & /*positionOffset*/,
      const Eigen::Matrix3d & /*positionCovariance*/) {
    OKVIS_THROW(Exception, "not implemented");
  }

  /*
  */
  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*/) {
    OKVIS_THROW(Exception, "not implemented");
  }

  /*
  */
  virtual void addMagnetometerMeasurement(
      const okvis::Time & /*stamp*/,
      const Eigen::Vector3d & /*fluxDensityMeas*/, double /*stdev*/) {
    OKVIS_THROW(Exception, "not implemented");
  }

  /*
  */
  virtual void addBarometerMeasurement(const okvis::Time & /*stamp*/,
                                       double /*staticPressure*/,
                                       double /*stdev*/) {
    OKVIS_THROW(Exception, "not implemented");
  }

  /*
  */
  virtual void addDifferentialPressureMeasurement(
      const okvis::Time & /*stamp*/, double /*differentialPressure*/,
      double /*stdev*/) {
    OKVIS_THROW(Exception, "not implemented");
  }

  bool addEigenImage(const okvis::Time & stamp, size_t cameraIndex,
                     const EigenImage & image);


  virtual void setStateCallback(const StateCallback & stateCallback);

  virtual void setFullStateCallback(
      const FullStateCallback & fullStateCallback);

  virtual void setFullStateCallbackWithExtrinsics(
      const FullStateCallbackWithExtrinsics & fullStateCallbackWithExtrinsics);

  virtual void setLandmarksCallback(
      const LandmarksCallback & landmarksCallback);

  virtual void setBlocking(bool blocking);


 protected:

  bool writeImuCsvDescription();
  bool writePosCsvDescription();
  bool writeMagCsvDescription();
  bool writeTracksCsvDescription(size_t cameraId);

  StateCallback stateCallback_; 
  FullStateCallback fullStateCallback_; 
  FullStateCallbackWithExtrinsics fullStateCallbackWithExtrinsics_; 
  LandmarksCallback landmarksCallback_; 
  std::shared_ptr<std::fstream> csvImuFile_;  
  std::shared_ptr<std::fstream> csvPosFile_;  
  std::shared_ptr<std::fstream> csvMagFile_;  
  typedef std::map<size_t, std::shared_ptr<std::fstream>> FilePtrMap;
  FilePtrMap csvTracksFiles_; 
  bool blocking_; 
};

}  // namespace okvis

#endif /* INCLUDE_OKVIS_VIOINTERFACE_HPP_ */