OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
VioBackendInterface.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: Aug 21, 2014
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  * Modified: Andreas Forster (an.forster@gmail.com)
32  *********************************************************************************/
33 
41 #ifndef INCLUDE_OKVIS_VIOBACKENDINTERFACE_HPP_
42 #define INCLUDE_OKVIS_VIOBACKENDINTERFACE_HPP_
43 
44 #include <memory>
45 
46 #include <ceres/ceres.h>
47 
48 #include <okvis/assert_macros.hpp>
50 
51 #include <okvis/MultiFrame.hpp>
52 #include <okvis/FrameTypedefs.hpp>
53 #include <okvis/Measurements.hpp>
54 #include <okvis/Parameters.hpp>
55 #include <okvis/Variables.hpp>
59 #include <okvis/ceres/Map.hpp>
62 
64 namespace okvis {
65 
68  public:
69  OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error)
70  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
71 
74  virtual ~VioBackendInterface() {}
75 
78  NotStarted = 0,
79  Ongoing = 1,
80  Complete = 2
81  };
82 
85 
90  virtual int addCamera(
91  const ExtrinsicsEstimationParameters & extrinsicsEstimationParameters) = 0;
92 
99  virtual int addImu(const ImuParameters & imuParameters) = 0;
100 
104  virtual void clearCameras() = 0;
105 
109  virtual void clearImus() = 0;
110 
112 
121  const okvis::ImuMeasurementDeque & imuMeasurements,
122  bool asKeyframe) = 0;
123 
130  virtual bool addLandmark(uint64_t landmarkId,
131  const Eigen::Vector4d & landmark) = 0;
132 
141  virtual bool removeObservation(uint64_t landmarkId, uint64_t poseId, size_t camIdx,
142  size_t keypointIdx) = 0;
149  virtual void optimize(size_t numIter, size_t numThreads = 1, bool verbose = false) = 0;
150 
158  virtual bool setOptimizationTimeLimit(double timeLimit, int minIterations) = 0;
159 
165  virtual bool isLandmarkAdded(uint64_t landmarkId) const = 0;
166 
172  virtual bool isLandmarkInitialized(uint64_t landmarkId) const = 0;
173 
176 
182  virtual bool getLandmark(uint64_t landmarkId, MapPoint& mapPoint) const = 0;
183 
189  virtual size_t getLandmarks(PointMap & landmarks) const = 0; // return a copy for thread safety
190 
196  virtual okvis::MultiFramePtr multiFrame(uint64_t frameId) const = 0;
197 
204  virtual bool get_T_WS(uint64_t poseId, okvis::kinematics::Transformation & T_WS) const = 0;
205 
213  virtual bool getSpeedAndBias(uint64_t poseId, uint64_t imuIdx, okvis::SpeedAndBias & speedAndBias) const = 0;
214 
222  virtual bool getCameraSensorStates(uint64_t poseId, size_t cameraIdx,
223  okvis::kinematics::Transformation & T_SCi) const = 0;
224 
227  virtual size_t numFrames() const = 0;
228 
231  virtual size_t numLandmarks() const = 0;
232 
235  virtual uint64_t currentFrameId() const = 0;
236 
239  OKVIS_THROW(Exception,"Uncertainty computation not implemented");
241  }
242 
244 
249  virtual bool isKeyframe(uint64_t frameId) const = 0;
250 
253 
258  virtual okvis::Time timestamp(uint64_t frameId) const = 0;
259 
265  virtual bool getPoseUncertainty(Eigen::Matrix<double,6,6> & /*P_T_WS*/) const {
266  OKVIS_THROW(Exception,"Uncertainty computation not implemented");
267  return false;
268  }
269 
275  virtual bool getStateUncertainty(Eigen::Matrix<double,15,15> & /*P*/) const {
276  OKVIS_THROW(Exception,"Uncertainty computation not implemented");
277  return false;
278  }
279 
283 
289  virtual bool set_T_WS(uint64_t poseId, const okvis::kinematics::Transformation & T_WS) = 0;
290 
298  virtual bool setSpeedAndBias(uint64_t poseId, size_t imuIdx, const okvis::SpeedAndBias & speedAndBias) = 0;
299 
307  virtual bool setCameraSensorStates(uint64_t poseId, size_t cameraIdx,
308  const okvis::kinematics::Transformation & T_SCi) = 0;
309 
314  virtual bool setLandmark(uint64_t landmarkId,
315  const Eigen::Vector4d & landmark) = 0;
316 
320  virtual void setLandmarkInitialized(uint64_t landmarkId, bool initialized) = 0;
321 
325  virtual void setKeyframe(uint64_t frameId, bool isKeyframe) = 0;
326 
329  virtual void setMap(std::shared_ptr<okvis::ceres::Map> mapPtr) = 0;
330 
333  virtual void setComputeUncertainty(bool /*computeUncertainty*/) {}
334 
336 };
337 
338 } // namespace okvis
339 
340 #endif /* INCLUDE_OKVIS_VIOBACKENDINTERFACE_HPP_ */
Struct to define the behavior of the camera extrinsics.
Definition: Parameters.hpp:60
virtual void setLandmarkInitialized(uint64_t landmarkId, bool initialized)=0
Set the landmark initialization state.
A type to store information about a point in the world map.
Definition: FrameTypedefs.hpp:139
virtual bool setOptimizationTimeLimit(double timeLimit, int minIterations)=0
Set a time limit for the optimization process.
std::deque< ImuMeasurement, Eigen::aligned_allocator< ImuMeasurement > > ImuMeasurementDeque
Definition: Measurements.hpp:163
virtual bool isKeyframe(uint64_t frameId) const =0
Checks if a particular frame is a keyframe.
virtual int addCamera(const ExtrinsicsEstimationParameters &extrinsicsEstimationParameters)=0
Add a camera to the configuration. Sensors can only be added and never removed.
Header file for the PoseParameterBlock class.
virtual bool getLandmark(uint64_t landmarkId, MapPoint &mapPoint) const =0
Get a specific landmark.
Header file for the MultiFrame class.
Header file for the Map class. This essentially encapsulates the ceres::Problem.
Eigen::Matrix< double, 9, 1 > SpeedAndBias
Definition: FrameTypedefs.hpp:234
std::shared_ptr< MultiFrame > MultiFramePtr
For convenience.
Definition: MultiFrame.hpp:272
This file contains the templated measurement structs, structs encapsulating Sensor data and related t...
virtual bool getStateUncertainty(Eigen::Matrix< double, 15, 15 > &) const
Get the current frame state uncertainty. param[out] P_T_WS Current pose uncertainty w...
Definition: VioBackendInterface.hpp:275
A class that does homogeneous transformations. This relates a frame A and B: T_AB; it consists of tra...
Definition: Transformation.hpp:76
virtual void setComputeUncertainty(bool)
Request computation of uncertainties. param[in] computeUncertainty True, if uncertainties should be c...
Definition: VioBackendInterface.hpp:333
virtual bool isLandmarkInitialized(uint64_t landmarkId) const =0
Checks whether the landmark is initialized.
Header file for the ReprojectionError class.
Header file for the HomogeneousPointParameterBlock class.
#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 bool getCameraSensorStates(uint64_t poseId, size_t cameraIdx, okvis::kinematics::Transformation &T_SCi) const =0
Get camera states for a given pose ID.
virtual bool getPoseUncertainty(Eigen::Matrix< double, 6, 6 > &) const
Get the current frame pose uncertainty. param[out] P_T_WS Current pose uncertainty w...
Definition: VioBackendInterface.hpp:265
virtual bool get_T_WS(uint64_t poseId, okvis::kinematics::Transformation &T_WS) const =0
Get pose for a given pose ID.
This file contains some useful assert macros.
Header file for the SpeedAndBiasParameterBlock class.
virtual size_t getLandmarks(PointMap &landmarks) const =0
Get a copy of all the landmarks as a PointMap.
virtual ~VioBackendInterface()
Definition: VioBackendInterface.hpp:74
virtual void clearCameras()=0
Remove all cameras from the configuration.
This file contains some typedefs related to state variables.
virtual okvis::MultiFramePtr multiFrame(uint64_t frameId) const =0
Get a multiframe.
virtual bool setLandmark(uint64_t landmarkId, const Eigen::Vector4d &landmark)=0
Set the homogeneous coordinates for a landmark.
virtual bool set_T_WS(uint64_t poseId, const okvis::kinematics::Transformation &T_WS)=0
Set pose for a given pose ID.
virtual InitializationStatus initializationStatus() const
Obtain the initialization status.
Definition: VioBackendInterface.hpp:238
virtual bool isLandmarkAdded(uint64_t landmarkId) const =0
Checks whether the landmark is added to the estimator.
InitializationStatus
Enum to define the status of initialization.
Definition: VioBackendInterface.hpp:77
virtual void clearImus()=0
Remove all IMUs from the configuration.
IMU parameters.
Definition: Parameters.hpp:105
Header file for the Transformation class.
std::map< uint64_t, MapPoint, std::less< uint64_t >, Eigen::aligned_allocator< MapPoint > > PointMap
Definition: FrameTypedefs.hpp:174
virtual bool setCameraSensorStates(uint64_t poseId, size_t cameraIdx, const okvis::kinematics::Transformation &T_SCi)=0
Set the transformation from sensor to camera frame for a given pose ID.
virtual size_t numFrames() const =0
Get the number of states/frames in the estimator.
virtual void setKeyframe(uint64_t frameId, bool isKeyframe)=0
Set whether a frame is a keyframe or not.
virtual bool addLandmark(uint64_t landmarkId, const Eigen::Vector4d &landmark)=0
Add a landmark.
Time representation. May either represent wall clock time or ROS clock time.
Definition: Time.hpp:187
virtual void setMap(std::shared_ptr< okvis::ceres::Map > mapPtr)=0
Set ceres map.
This file contains struct definitions that encapsulate parameters and settings.
virtual size_t numLandmarks() const =0
Get the number of landmarks in the backend.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VioBackendInterface()
Default constructor.
Definition: VioBackendInterface.hpp:73
virtual bool getSpeedAndBias(uint64_t poseId, uint64_t imuIdx, okvis::SpeedAndBias &speedAndBias) const =0
Get speeds and IMU biases for a given pose ID.
virtual bool setSpeedAndBias(uint64_t poseId, size_t imuIdx, const okvis::SpeedAndBias &speedAndBias)=0
Set the speeds and IMU biases for a given pose ID.
virtual uint64_t currentFrameId() const =0
Get the ID of the newest frame added to the state.
virtual bool removeObservation(uint64_t landmarkId, uint64_t poseId, size_t camIdx, size_t keypointIdx)=0
Remove an observation from a landmark, if available.
virtual int addImu(const ImuParameters &imuParameters)=0
Add an IMU to the configuration.
virtual okvis::Time timestamp(uint64_t frameId) const =0
Get the timestamp for a particular frame.
virtual void optimize(size_t numIter, size_t numThreads=1, bool verbose=false)=0
Start optimization.
Header file for the MarginalizationError class.
An abstract interface for backends.
Definition: VioBackendInterface.hpp:67
This file contains useful typedefs and structs related to frames.
virtual bool addStates(okvis::MultiFramePtr multiFrame, const okvis::ImuMeasurementDeque &imuMeasurements, bool asKeyframe)=0
Add a pose to the state.