OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Public Types | Public Member Functions | List of all members
okvis::VioBackendInterface Class Referenceabstract

An abstract interface for backends. More...

#include <VioBackendInterface.hpp>

Inheritance diagram for okvis::VioBackendInterface:
okvis::Estimator okvis::MockVioBackendInterface

Public Types

enum  InitializationStatus { InitializationStatus::NotStarted = 0, InitializationStatus::Ongoing = 1, InitializationStatus::Complete = 2 }
 Enum to define the status of initialization. More...
 

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW VioBackendInterface ()
 Default constructor. More...
 
virtual ~VioBackendInterface ()
 
virtual bool addStates (okvis::MultiFramePtr multiFrame, const okvis::ImuMeasurementDeque &imuMeasurements, bool asKeyframe)=0
 Add a pose to the state. More...
 
virtual bool addLandmark (uint64_t landmarkId, const Eigen::Vector4d &landmark)=0
 Add a landmark. More...
 
virtual bool removeObservation (uint64_t landmarkId, uint64_t poseId, size_t camIdx, size_t keypointIdx)=0
 Remove an observation from a landmark, if available. More...
 
virtual void optimize (size_t numIter, size_t numThreads=1, bool verbose=false)=0
 Start optimization. More...
 
virtual bool setOptimizationTimeLimit (double timeLimit, int minIterations)=0
 Set a time limit for the optimization process. More...
 
virtual bool isLandmarkAdded (uint64_t landmarkId) const =0
 Checks whether the landmark is added to the estimator. More...
 
virtual bool isLandmarkInitialized (uint64_t landmarkId) const =0
 Checks whether the landmark is initialized. More...
 
virtual bool isKeyframe (uint64_t frameId) const =0
 Checks if a particular frame is a keyframe. More...
 
Sensor configuration related
virtual int addCamera (const ExtrinsicsEstimationParameters &extrinsicsEstimationParameters)=0
 Add a camera to the configuration. Sensors can only be added and never removed. More...
 
virtual int addImu (const ImuParameters &imuParameters)=0
 Add an IMU to the configuration. More...
 
virtual void clearCameras ()=0
 Remove all cameras from the configuration. More...
 
virtual void clearImus ()=0
 Remove all IMUs from the configuration. More...
 
Getters
virtual bool getLandmark (uint64_t landmarkId, MapPoint &mapPoint) const =0
 Get a specific landmark. More...
 
virtual size_t getLandmarks (PointMap &landmarks) const =0
 Get a copy of all the landmarks as a PointMap. More...
 
virtual okvis::MultiFramePtr multiFrame (uint64_t frameId) const =0
 Get a multiframe. More...
 
virtual bool get_T_WS (uint64_t poseId, okvis::kinematics::Transformation &T_WS) const =0
 Get pose for a given pose ID. More...
 
virtual bool getSpeedAndBias (uint64_t poseId, uint64_t imuIdx, okvis::SpeedAndBias &speedAndBias) const =0
 Get speeds and IMU biases for a given pose ID. More...
 
virtual bool getCameraSensorStates (uint64_t poseId, size_t cameraIdx, okvis::kinematics::Transformation &T_SCi) const =0
 Get camera states for a given pose ID. More...
 
virtual size_t numFrames () const =0
 Get the number of states/frames in the estimator. More...
 
virtual size_t numLandmarks () const =0
 Get the number of landmarks in the backend. More...
 
virtual uint64_t currentFrameId () const =0
 Get the ID of the newest frame added to the state. More...
 
virtual InitializationStatus initializationStatus () const
 Obtain the initialization status. More...
 
virtual okvis::Time timestamp (uint64_t frameId) const =0
 Get the timestamp for a particular frame. More...
 
virtual bool getPoseUncertainty (Eigen::Matrix< double, 6, 6 > &) const
 Get the current frame pose uncertainty. param[out] P_T_WS Current pose uncertainty w.r.t. [r_WS,delta_alpha_WS]. More...
 
virtual bool getStateUncertainty (Eigen::Matrix< double, 15, 15 > &) const
 Get the current frame state uncertainty. param[out] P_T_WS Current pose uncertainty w.r.t. [r_WS,delta_alpha_WS,v_W,b_g,b_a]. More...
 
Setters
virtual bool set_T_WS (uint64_t poseId, const okvis::kinematics::Transformation &T_WS)=0
 Set pose for a given pose ID. More...
 
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. More...
 
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. More...
 
virtual bool setLandmark (uint64_t landmarkId, const Eigen::Vector4d &landmark)=0
 Set the homogeneous coordinates for a landmark. More...
 
virtual void setLandmarkInitialized (uint64_t landmarkId, bool initialized)=0
 Set the landmark initialization state. More...
 
virtual void setKeyframe (uint64_t frameId, bool isKeyframe)=0
 Set whether a frame is a keyframe or not. More...
 
virtual void setMap (std::shared_ptr< okvis::ceres::Map > mapPtr)=0
 Set ceres map. More...
 
virtual void setComputeUncertainty (bool)
 Request computation of uncertainties. param[in] computeUncertainty True, if uncertainties should be computed. More...
 

Detailed Description

An abstract interface for backends.

Member Enumeration Documentation

Enum to define the status of initialization.

Enumerator
NotStarted 

Initialization not started.

Ongoing 

Initialization ongoing.

Complete 

Initialization complete.

Constructor & Destructor Documentation

EIGEN_MAKE_ALIGNED_OPERATOR_NEW okvis::VioBackendInterface::VioBackendInterface ( )
inline

Default constructor.

virtual okvis::VioBackendInterface::~VioBackendInterface ( )
inlinevirtual

Member Function Documentation

virtual int okvis::VioBackendInterface::addCamera ( const ExtrinsicsEstimationParameters extrinsicsEstimationParameters)
pure virtual

Add a camera to the configuration. Sensors can only be added and never removed.

Parameters
extrinsicsEstimationParametersThe parameters that tell how to estimate extrinsics.
Returns
Index of new camera.

Implemented in okvis::Estimator.

virtual int okvis::VioBackendInterface::addImu ( const ImuParameters imuParameters)
pure virtual

Add an IMU to the configuration.

Warning
Currently there is only one IMU supported.
Parameters
imuParametersThe IMU parameters.
Returns
index of IMU.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::addLandmark ( uint64_t  landmarkId,
const Eigen::Vector4d &  landmark 
)
pure virtual

Add a landmark.

Parameters
landmarkIdID of the new landmark.
landmarkHomogeneous coordinates of landmark in W-frame.
Returns
True if successful.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::addStates ( okvis::MultiFramePtr  multiFrame,
const okvis::ImuMeasurementDeque imuMeasurements,
bool  asKeyframe 
)
pure virtual

Add a pose to the state.

Parameters
multiFrameMatched multiFrame.
imuMeasurementsIMU measurements from last state to new one.
asKeyframeIs this new frame a keyframe?
Returns
True if successful.

Implemented in okvis::Estimator.

virtual void okvis::VioBackendInterface::clearCameras ( )
pure virtual

Remove all cameras from the configuration.

Implemented in okvis::Estimator.

virtual void okvis::VioBackendInterface::clearImus ( )
pure virtual

Remove all IMUs from the configuration.

Implemented in okvis::Estimator.

virtual uint64_t okvis::VioBackendInterface::currentFrameId ( ) const
pure virtual

Get the ID of the newest frame added to the state.

Returns
The ID of the current frame.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::get_T_WS ( uint64_t  poseId,
okvis::kinematics::Transformation T_WS 
) const
pure virtual

Get pose for a given pose ID.

Parameters
[in]poseIdID of desired pose.
[out]T_WSHomogeneous transformation of this pose.
Returns
True if successful.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::getCameraSensorStates ( uint64_t  poseId,
size_t  cameraIdx,
okvis::kinematics::Transformation T_SCi 
) const
pure virtual

Get camera states for a given pose ID.

Parameters
[in]poseIdID of pose to get camera state for.
[in]cameraIdxindex of camera to get state for.
[out]T_SCiHomogeneous transformation from sensor (IMU) frame to camera frame.
Returns
True if successful.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::getLandmark ( uint64_t  landmarkId,
MapPoint mapPoint 
) const
pure virtual

Get a specific landmark.

Parameters
[in]landmarkIdID of desired landmark.
[out]mapPointLandmark information, such as quality, coordinates etc.
Returns
True if successful.

Implemented in okvis::Estimator.

virtual size_t okvis::VioBackendInterface::getLandmarks ( PointMap landmarks) const
pure virtual

Get a copy of all the landmarks as a PointMap.

Parameters
[out]landmarksThe landmarks.
Returns
number of landmarks.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::getPoseUncertainty ( Eigen::Matrix< double, 6, 6 > &  ) const
inlinevirtual

Get the current frame pose uncertainty. param[out] P_T_WS Current pose uncertainty w.r.t. [r_WS,delta_alpha_WS].

Returns
True on success.
virtual bool okvis::VioBackendInterface::getSpeedAndBias ( uint64_t  poseId,
uint64_t  imuIdx,
okvis::SpeedAndBias speedAndBias 
) const
pure virtual

Get speeds and IMU biases for a given pose ID.

Parameters
[in]poseIdID of pose to get speeds and biases for.
[in]imuIdxindex of IMU to get biases for.
[out]speedAndBiasSpeed And bias requested.
Returns
True if successful.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::getStateUncertainty ( Eigen::Matrix< double, 15, 15 > &  ) const
inlinevirtual

Get the current frame state uncertainty. param[out] P_T_WS Current pose uncertainty w.r.t. [r_WS,delta_alpha_WS,v_W,b_g,b_a].

Returns
True on success.
virtual InitializationStatus okvis::VioBackendInterface::initializationStatus ( ) const
inlinevirtual

Obtain the initialization status.

virtual bool okvis::VioBackendInterface::isKeyframe ( uint64_t  frameId) const
pure virtual

Checks if a particular frame is a keyframe.

Parameters
[in]frameIdID of frame to check.
Returns
True if the frame is a keyframe.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::isLandmarkAdded ( uint64_t  landmarkId) const
pure virtual

Checks whether the landmark is added to the estimator.

Parameters
landmarkIdThe ID.
Returns
True if added.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::isLandmarkInitialized ( uint64_t  landmarkId) const
pure virtual

Checks whether the landmark is initialized.

Parameters
landmarkIdThe ID.
Returns
True if initialised.

Implemented in okvis::Estimator.

virtual okvis::MultiFramePtr okvis::VioBackendInterface::multiFrame ( uint64_t  frameId) const
pure virtual

Get a multiframe.

Parameters
frameIdID of desired multiframe.
Returns
Shared pointer to multiframe.

Implemented in okvis::Estimator.

virtual size_t okvis::VioBackendInterface::numFrames ( ) const
pure virtual

Get the number of states/frames in the estimator.

Returns
The number of frames.

Implemented in okvis::Estimator.

virtual size_t okvis::VioBackendInterface::numLandmarks ( ) const
pure virtual

Get the number of landmarks in the backend.

Returns
The number of landmarks.

Implemented in okvis::Estimator.

virtual void okvis::VioBackendInterface::optimize ( size_t  numIter,
size_t  numThreads = 1,
bool  verbose = false 
)
pure virtual

Start optimization.

Parameters
[in]numIterMaximum number of iterations.
[in]numThreadsNumber of threads.
[in]verbosePrint out optimization progress and result, if true.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::removeObservation ( uint64_t  landmarkId,
uint64_t  poseId,
size_t  camIdx,
size_t  keypointIdx 
)
pure virtual

Remove an observation from a landmark, if available.

Parameters
landmarkIdID of landmark.
poseIdID of pose where the landmark was observed.
camIdxID of camera frame where the landmark was observed.
keypointIdxID of keypoint corresponding to the landmark.
Returns
True if observation was present and successfully removed.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::set_T_WS ( uint64_t  poseId,
const okvis::kinematics::Transformation T_WS 
)
pure virtual

Set pose for a given pose ID.

Parameters
[in]poseIdID of the pose that should be changed.
[in]T_WSnew homogeneous transformation.
Returns
True if successful.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::setCameraSensorStates ( uint64_t  poseId,
size_t  cameraIdx,
const okvis::kinematics::Transformation T_SCi 
)
pure virtual

Set the transformation from sensor to camera frame for a given pose ID.

Parameters
[in]poseIdID of the pose to change corresponding camera states for.
[in]cameraIdxIndex of camera to set state for.
[in]T_SCinew homogeneous transformation from sensor (IMU) to camera frame.
Returns
True if successful.

Implemented in okvis::Estimator.

virtual void okvis::VioBackendInterface::setComputeUncertainty ( bool  )
inlinevirtual

Request computation of uncertainties. param[in] computeUncertainty True, if uncertainties should be computed.

virtual void okvis::VioBackendInterface::setKeyframe ( uint64_t  frameId,
bool  isKeyframe 
)
pure virtual

Set whether a frame is a keyframe or not.

Parameters
[in]frameIdThe frame ID.
[in]isKeyframeWhether or not keyrame.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::setLandmark ( uint64_t  landmarkId,
const Eigen::Vector4d &  landmark 
)
pure virtual

Set the homogeneous coordinates for a landmark.

Parameters
[in]landmarkIdThe landmark ID.
[in]landmarkHomogeneous coordinates of landmark in W-frame.
Returns
True if successful.

Implemented in okvis::Estimator.

virtual void okvis::VioBackendInterface::setLandmarkInitialized ( uint64_t  landmarkId,
bool  initialized 
)
pure virtual

Set the landmark initialization state.

Parameters
[in]landmarkIdThe landmark ID.
[in]initializedWhether or not initialised.

Implemented in okvis::Estimator.

virtual void okvis::VioBackendInterface::setMap ( std::shared_ptr< okvis::ceres::Map mapPtr)
pure virtual

Set ceres map.

Parameters
[in]mapPtrThe pointer to the okvis::ceres::Map.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::setOptimizationTimeLimit ( double  timeLimit,
int  minIterations 
)
pure virtual

Set a time limit for the optimization process.

Parameters
[in]timeLimitTime limit in seconds. If timeLimit < 0 the time limit is removed.
[in]minIterationsminimum iterations the optimization process should do disregarding the time limit.
Returns
True if successful.

Implemented in okvis::Estimator.

virtual bool okvis::VioBackendInterface::setSpeedAndBias ( uint64_t  poseId,
size_t  imuIdx,
const okvis::SpeedAndBias speedAndBias 
)
pure virtual

Set the speeds and IMU biases for a given pose ID.

Parameters
[in]poseIdID of the pose to change corresponding speeds and biases for.
[in]imuIdxindex of IMU to get biases for. As only one IMU is supported this is always 0.
[in]speedAndBiasnew speeds and biases.
Returns
True if successful.

Implemented in okvis::Estimator.

virtual okvis::Time okvis::VioBackendInterface::timestamp ( uint64_t  frameId) const
pure virtual

Get the timestamp for a particular frame.

Parameters
[in]frameIdID of frame.
Returns
Timestamp of frame.

Implemented in okvis::Estimator.


The documentation for this class was generated from the following file: