OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
VioInterface.hpp
Go to the documentation of this file.
1 /*********************************************************************************
2  * OKVIS - Open Keyframe-based Visual-Inertial SLAM
3  * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
14  * its contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Created on: Jun 11, 2013
30  * Author: Paul Furgale
31  * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
32  * Modified: Andreas Forster (an.forster@gmail.com)
33  *********************************************************************************/
34 
43 #ifndef INCLUDE_OKVIS_VIOINTERFACE_HPP_
44 #define INCLUDE_OKVIS_VIOINTERFACE_HPP_
45 
46 #include <cstdint>
47 #include <memory>
48 #include <functional>
49 
50 #pragma GCC diagnostic push
51 #pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
52 #include <opencv2/core/core.hpp>
53 #include <opencv2/features2d/features2d.hpp>
54 #pragma GCC diagnostic pop
55 #include <okvis/assert_macros.hpp>
56 #include <okvis/Time.hpp>
57 #include <okvis/FrameTypedefs.hpp>
59 
61 namespace okvis {
62 
66 class VioInterface {
67  public:
68  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
69 
70  typedef std::function<
72  typedef std::function<
73  void(const okvis::Time &, const okvis::kinematics::Transformation &,
74  const Eigen::Matrix<double, 9, 1> &,
75  const Eigen::Matrix<double, 3, 1> &)> FullStateCallback;
76  typedef std::function<
77  void(
78  const okvis::Time &,
80  const Eigen::Matrix<double, 9, 1> &,
81  const Eigen::Matrix<double, 3, 1> &,
82  const std::vector<okvis::kinematics::Transformation,
83  Eigen::aligned_allocator<okvis::kinematics::Transformation> >&)> FullStateCallbackWithExtrinsics;
84  typedef Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic> EigenImage;
85  typedef std::function<
86  void(const okvis::Time &, const okvis::MapPointVector &,
88 
89  VioInterface();
90  virtual ~VioInterface();
91 
94 
97  bool setImuCsvFile(std::fstream& csvFile);
100  bool setImuCsvFile(const std::string& csvFileName);
101 
105  bool setTracksCsvFile(size_t cameraId, std::fstream& csvFile);
109  bool setTracksCsvFile(size_t cameraId, const std::string& csvFileName);
110 
113  bool setPosCsvFile(std::fstream& csvFile);
116  bool setPosCsvFile(const std::string& csvFileName);
117 
120  bool setMagCsvFile(std::fstream& csvFile);
123  bool setMagCsvFile(const std::string& csvFileName);
124 
128 
139  virtual bool addImage(const okvis::Time & stamp, size_t cameraIndex,
140  const cv::Mat & image,
141  const std::vector<cv::KeyPoint> * keypoints = 0,
142  bool* asKeyframe = 0) = 0;
143 
154  virtual bool addKeypoints(const okvis::Time & stamp, size_t cameraIndex,
155  const std::vector<cv::KeyPoint> & keypoints,
156  const std::vector<uint64_t> & landmarkIds,
157  const cv::Mat& descriptors = cv::Mat(),
158  bool* asKeyframe = 0) = 0;
159 
164  virtual bool addImuMeasurement(const okvis::Time & stamp,
165  const Eigen::Vector3d & alpha,
166  const Eigen::Vector3d & omega) = 0;
167 
170  /*
174  */
176  const okvis::Time & /*stamp*/, const Eigen::Vector3d & /*position*/,
177  const Eigen::Vector3d & /*positionOffset*/,
178  const Eigen::Matrix3d & /*positionCovariance*/) {
179  OKVIS_THROW(Exception, "not implemented");
180  }
181 
184  /*
191  */
192  virtual void addGpsMeasurement(
193  const okvis::Time & /*stamp*/, double /*lat_wgs84_deg*/,
194  double /*lon_wgs84_deg*/, double /*alt_wgs84_deg*/,
195  const Eigen::Vector3d & /*positionOffset*/,
196  const Eigen::Matrix3d & /*positionCovarianceENU*/) {
197  OKVIS_THROW(Exception, "not implemented");
198  }
199 
202  /*
206  */
209  const okvis::Time & /*stamp*/,
210  const Eigen::Vector3d & /*fluxDensityMeas*/, double /*stdev*/) {
211  OKVIS_THROW(Exception, "not implemented");
212  }
213 
216  /*
220  */
221  virtual void addBarometerMeasurement(const okvis::Time & /*stamp*/,
222  double /*staticPressure*/,
223  double /*stdev*/) {
224  OKVIS_THROW(Exception, "not implemented");
225  }
226 
229  /*
233  */
235  const okvis::Time & /*stamp*/, double /*differentialPressure*/,
236  double /*stdev*/) {
237  OKVIS_THROW(Exception, "not implemented");
238  }
239 
247  bool addEigenImage(const okvis::Time & stamp, size_t cameraIndex,
248  const EigenImage & image);
249 
253 
260  virtual void setStateCallback(const StateCallback & stateCallback);
261 
269  virtual void setFullStateCallback(
270  const FullStateCallback & fullStateCallback);
271 
282  const FullStateCallbackWithExtrinsics & fullStateCallbackWithExtrinsics);
283 
289  virtual void setLandmarksCallback(
290  const LandmarksCallback & landmarksCallback);
291 
296  virtual void setBlocking(bool blocking);
297 
299 
300  protected:
301 
303  bool writeImuCsvDescription();
305  bool writePosCsvDescription();
307  bool writeMagCsvDescription();
309  bool writeTracksCsvDescription(size_t cameraId);
310 
315  std::shared_ptr<std::fstream> csvImuFile_;
316  std::shared_ptr<std::fstream> csvPosFile_;
317  std::shared_ptr<std::fstream> csvMagFile_;
318  typedef std::map<size_t, std::shared_ptr<std::fstream>> FilePtrMap;
320  bool blocking_;
321 };
322 
323 } // namespace okvis
324 
325 #endif /* INCLUDE_OKVIS_VIOINTERFACE_HPP_ */
std::shared_ptr< std::fstream > csvMagFile_
Magnetometer CSV File.
Definition: VioInterface.hpp:317
LandmarksCallback landmarksCallback_
Landmarks callback function.
Definition: VioInterface.hpp:314
Eigen::Matrix< unsigned char, Eigen::Dynamic, Eigen::Dynamic > EigenImage
Definition: VioInterface.hpp:84
FilePtrMap csvTracksFiles_
Tracks CSV Files.
Definition: VioInterface.hpp:319
FullStateCallbackWithExtrinsics fullStateCallbackWithExtrinsics_
Full state and extrinsics callback function.
Definition: VioInterface.hpp:313
virtual void addPositionMeasurement(const okvis::Time &, const Eigen::Vector3d &, const Eigen::Vector3d &, const Eigen::Matrix3d &)
Add a position measurement.
Definition: VioInterface.hpp:175
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
Add an abstracted image observation.
bool writeTracksCsvDescription(size_t cameraId)
Write first line of tracks (data associations) CSV file to describe columns.
Definition: VioInterface.cpp:144
bool addEigenImage(const okvis::Time &stamp, size_t cameraIndex, const EigenImage &image)
This is just handy for the python interface.
Definition: VioInterface.cpp:71
bool writeImuCsvDescription()
Write first line of IMU CSV file to describe columns.
Definition: VioInterface.cpp:109
std::map< size_t, std::shared_ptr< std::fstream > > FilePtrMap
Definition: VioInterface.hpp:318
virtual void setStateCallback(const StateCallback &stateCallback)
Set the stateCallback to be called every time a new state is estimated. When an implementing class ha...
Definition: VioInterface.cpp:80
bool setTracksCsvFile(size_t cameraId, std::fstream &csvFile)
Set a CVS file where the tracks (data associations) will be saved to.
Definition: VioInterface.cpp:210
virtual void setBlocking(bool blocking)
Set the blocking variable that indicates whether the addMeasurement() functions should return immedia...
Definition: VioInterface.cpp:104
virtual void addDifferentialPressureMeasurement(const okvis::Time &, double, double)
Add a differential pressure measurement.
Definition: VioInterface.hpp:234
bool writePosCsvDescription()
Write first line of position CSV file to describe columns.
Definition: VioInterface.cpp:122
FullStateCallback fullStateCallback_
Full state callback function.
Definition: VioInterface.hpp:312
std::function< void(const okvis::Time &, const okvis::kinematics::Transformation &, const Eigen::Matrix< double, 9, 1 > &, const Eigen::Matrix< double, 3, 1 > &)> FullStateCallback
Definition: VioInterface.hpp:75
A class that does homogeneous transformations. This relates a frame A and B: T_AB; it consists of tra...
Definition: Transformation.hpp:76
Header file for the TimeBase, Time and WallTime class.
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
Definition: VioInterface.hpp:83
std::function< void(const okvis::Time &, const okvis::MapPointVector &, const okvis::MapPointVector &)> LandmarksCallback
Definition: VioInterface.hpp:87
std::vector< MapPoint, Eigen::aligned_allocator< MapPoint > > MapPointVector
Definition: FrameTypedefs.hpp:172
virtual ~VioInterface()
Definition: VioInterface.cpp:55
bool setMagCsvFile(std::fstream &csvFile)
Set a CVS file where the magnetometer measurements will be saved to.
Definition: VioInterface.cpp:192
#define OKVIS_THROW(exceptionType, message)
Definition: assert_macros.hpp:98
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
virtual void setLandmarksCallback(const LandmarksCallback &landmarksCallback)
Set the landmarksCallback to be called every time a new state is estimated. When an implementing clas...
Definition: VioInterface.cpp:97
This file contains some useful assert macros.
bool writeMagCsvDescription()
Write first line of magnetometer CSV file to describe columns.
Definition: VioInterface.cpp:133
std::shared_ptr< std::fstream > csvImuFile_
IMU CSV file.
Definition: VioInterface.hpp:315
virtual void setFullStateCallback(const FullStateCallback &fullStateCallback)
Set the fullStateCallback to be called every time a new state is estimated. When an implementing clas...
Definition: VioInterface.cpp:85
bool setPosCsvFile(std::fstream &csvFile)
Set a CVS file where the position measurements will be saved to.
Definition: VioInterface.cpp:174
bool blocking_
Blocking option. Whether the addMeasurement() functions should wait until proccessing is complete...
Definition: VioInterface.hpp:320
StateCallback stateCallback_
State callback function.
Definition: VioInterface.hpp:311
An abstract base class for interfaces between Front- and Backend.
Definition: VioInterface.hpp:66
bool setImuCsvFile(std::fstream &csvFile)
Set a CVS file where the IMU data will be saved to.
Definition: VioInterface.cpp:156
Header file for the Transformation class.
VioInterface()
Definition: VioInterface.cpp:53
std::function< void(const okvis::Time &, const okvis::kinematics::Transformation &)> StateCallback
Definition: VioInterface.hpp:71
virtual void addGpsMeasurement(const okvis::Time &, double, double, double, const Eigen::Vector3d &, const Eigen::Matrix3d &)
Add a position measurement.
Definition: VioInterface.hpp:192
virtual void addBarometerMeasurement(const okvis::Time &, double, double)
Add a static pressure measurement.
Definition: VioInterface.hpp:221
Time representation. May either represent wall clock time or ROS clock time.
Definition: Time.hpp:187
virtual bool addImuMeasurement(const okvis::Time &stamp, const Eigen::Vector3d &alpha, const Eigen::Vector3d &omega)=0
Add an IMU measurement.
virtual void setFullStateCallbackWithExtrinsics(const FullStateCallbackWithExtrinsics &fullStateCallbackWithExtrinsics)
Set the fullStateCallbackWithExtrinsics to be called every time a new state is estimated. When an implementing class has an estimate, they can call: _fullStateCallbackWithEctrinsics( stamp, T_w_vk, speedAndBiases, omega_S, vector_of_T_SCi); where stamp is the timestamp and T_w_vk is the transformation (and uncertainty) that transforms points from the vehicle frame to the world frame. speedAndBiases contain speed in world frame followed by gyro and acc biases. omega_S is the rotation speed vector_of_T_SCi contains the (uncertain) transformations of extrinsics T_SCi.
Definition: VioInterface.cpp:91
std::shared_ptr< std::fstream > csvPosFile_
Position CSV File.
Definition: VioInterface.hpp:316
virtual void addMagnetometerMeasurement(const okvis::Time &, const Eigen::Vector3d &, double)
Add a magnetometer measurement.
Definition: VioInterface.hpp:208
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
Add a new image.
This file contains useful typedefs and structs related to frames.