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

A frontend using BRISK features. More...

#include <Frontend.hpp>

Inheritance diagram for okvis::Frontend:
okvis::VioFrontendInterface

Public Types

typedef okvis::timing::Timer TimerSwitchable
 

Public Member Functions

 Frontend (size_t numCameras)
 Constructor. More...
 
virtual ~Frontend ()
 
virtual bool detectAndDescribe (size_t cameraIndex, std::shared_ptr< okvis::MultiFrame > frameOut, const okvis::kinematics::Transformation &T_WC, const std::vector< cv::KeyPoint > *keypoints)
 Detection and descriptor extraction on a per image basis. More...
 
virtual bool dataAssociationAndInitialization (okvis::Estimator &estimator, okvis::kinematics::Transformation &T_WS_propagated, const okvis::VioParameters &params, const std::shared_ptr< okvis::MapPointVector > map, std::shared_ptr< okvis::MultiFrame > framesInOut, bool *asKeyframe)
 Matching as well as initialization of landmarks and state. More...
 
virtual bool propagation (const okvis::ImuMeasurementDeque &imuMeasurements, const okvis::ImuParameters &imuParams, okvis::kinematics::Transformation &T_WS_propagated, okvis::SpeedAndBias &speedAndBiases, const okvis::Time &t_start, const okvis::Time &t_end, Eigen::Matrix< double, 15, 15 > *covariance, Eigen::Matrix< double, 15, 15 > *jacobian) const
 Propagates pose, speeds and biases with given IMU measurements. More...
 
Getters related to the BRISK detector
size_t getBriskDetectionOctaves () const
 Get the number of octaves of the BRISK detector. More...
 
double getBriskDetectionThreshold () const
 Get the detection threshold of the BRISK detector. More...
 
double getBriskDetectionAbsoluteThreshold () const
 Get the absolute threshold of the BRISK detector. More...
 
size_t getBriskDetectionMaximumKeypoints () const
 Get the maximum amount of keypoints of the BRISK detector. More...
 
Getters related to the BRISK descriptor
bool getBriskDescriptionRotationInvariance () const
 Get the rotation invariance setting of the BRISK descriptor. More...
 
bool getBriskDescriptionScaleInvariance () const
 Get the scale invariance setting of the BRISK descriptor. More...
 
Other getters
double getBriskMatchingThreshold () const
 Get the matching threshold. More...
 
float getKeyframeInsertionOverlapThershold () const
 Get the area overlap threshold under which a new keyframe is inserted. More...
 
float getKeyframeInsertionMatchingRatioThreshold () const
 Get the matching ratio threshold under which a new keyframe is inserted. More...
 
bool isInitialized ()
 Returns true if the initialization has been completed (RANSAC with actual translation) More...
 
Setters related to the BRISK detector
void setBriskDetectionOctaves (size_t octaves)
 Set the number of octaves of the BRISK detector. More...
 
void setBriskDetectionThreshold (double threshold)
 Set the detection threshold of the BRISK detector. More...
 
void setBriskDetectionAbsoluteThreshold (double threshold)
 Set the absolute threshold of the BRISK detector. More...
 
void setBriskDetectionMaximumKeypoints (size_t maxKeypoints)
 Set the maximum number of keypoints of the BRISK detector. More...
 
Setters related to the BRISK descriptor
void setBriskDescriptionRotationInvariance (bool invariance)
 Set the rotation invariance setting of the BRISK descriptor. More...
 
void setBriskDescriptionScaleInvariance (bool invariance)
 Set the scale invariance setting of the BRISK descriptor. More...
 
Other setters
void setBriskMatchingThreshold (double threshold)
 Set the matching threshold. More...
 
void setKeyframeInsertionOverlapThreshold (float threshold)
 Set the area overlap threshold under which a new keyframe is inserted. More...
 
void setKeyframeInsertionMatchingRatioThreshold (float threshold)
 Set the matching ratio threshold under which a new keyframe is inserted. More...
 
- Public Member Functions inherited from okvis::VioFrontendInterface
 VioFrontendInterface ()
 
virtual ~VioFrontendInterface ()
 

Private Member Functions

bool doWeNeedANewKeyframe (const okvis::Estimator &estimator, std::shared_ptr< okvis::MultiFrame > currentFrame)
 Decision whether a new frame should be keyframe or not. More...
 
template<class MATCHING_ALGORITHM >
int matchToKeyframes (okvis::Estimator &estimator, const okvis::VioParameters &params, const uint64_t currentFrameId, bool &rotationOnly, bool usePoseUncertainty=true, double *uncertainMatchFraction=0, bool removeOutliers=true)
 Match a new multiframe to existing keyframes. More...
 
template<class MATCHING_ALGORITHM >
int matchToLastFrame (okvis::Estimator &estimator, const okvis::VioParameters &params, const uint64_t currentFrameId, bool usePoseUncertainty=true, bool removeOutliers=true)
 Match a new multiframe to the last frame. More...
 
template<class MATCHING_ALGORITHM >
void matchStereo (okvis::Estimator &estimator, std::shared_ptr< okvis::MultiFrame > multiFrame)
 Match the frames inside the multiframe to each other to initialise new landmarks. More...
 
int runRansac3d2d (okvis::Estimator &estimator, const okvis::cameras::NCameraSystem &nCameraSystem, std::shared_ptr< okvis::MultiFrame > currentFrame, bool removeOutliers)
 Perform 3D/2D RANSAC. More...
 
int runRansac2d2d (okvis::Estimator &estimator, const okvis::VioParameters &params, uint64_t currentFrameId, uint64_t olderFrameId, bool initializePose, bool removeOutliers, bool &rotationOnly)
 Perform 2D/2D RANSAC. More...
 
void initialiseBriskFeatureDetectors ()
 (re)instantiates feature detectors and descriptor extractors. Used after settings changed or at startup. More...
 

Private Attributes

std::vector< std::shared_ptr
< cv::FeatureDetector > > 
featureDetectors_
 feature detectors with the current settings. The vector contains one for each camera to ensure that there are no problems with parallel detection. More...
 
std::vector< std::shared_ptr
< cv::DescriptorExtractor > > 
descriptorExtractors_
 feature descriptors with the current settings. The vector contains one for each camera to ensure that there are no problems with parallel detection. More...
 
std::vector< std::unique_ptr
< std::mutex > > 
featureDetectorMutexes_
 Mutexes for feature detectors and descriptors. More...
 
bool isInitialized_
 Is the pose initialised? More...
 
const size_t numCameras_
 Number of cameras in the configuration. More...
 
std::unique_ptr
< okvis::DenseMatcher
matcher_
 Matcher object. More...
 
float keyframeInsertionOverlapThreshold_
 If the hull-area around all matched keypoints of the current frame (with existing landmarks) divided by the hull-area around all keypoints in the current frame is lower than this threshold it should be a new keyframe. More...
 
float keyframeInsertionMatchingRatioThreshold_
 If the number of matched keypoints of the current frame with an older frame divided by the amount of points inside the convex hull around all keypoints is lower than the threshold it should be a keyframe. More...
 
BRISK detection parameters
size_t briskDetectionOctaves_
 The set number of brisk octaves. More...
 
double briskDetectionThreshold_
 The set BRISK detection threshold. More...
 
double briskDetectionAbsoluteThreshold_
 The set BRISK absolute detection threshold. More...
 
size_t briskDetectionMaximumKeypoints_
 The set maximum number of keypoints. More...
 
BRISK descriptor extractor parameters
bool briskDescriptionRotationInvariance_
 The set rotation invariance setting. More...
 
bool briskDescriptionScaleInvariance_
 The set scale invariance setting. More...
 
BRISK matching parameters
double briskMatchingThreshold_
 The set BRISK matching threshold. More...
 

Detailed Description

A frontend using BRISK features.

Member Typedef Documentation

Constructor & Destructor Documentation

okvis::Frontend::Frontend ( size_t  numCameras)

Constructor.

Parameters
numCamerasNumber of cameras in the sensor configuration.
virtual okvis::Frontend::~Frontend ( )
inlinevirtual

Member Function Documentation

bool okvis::Frontend::dataAssociationAndInitialization ( okvis::Estimator estimator,
okvis::kinematics::Transformation T_WS_propagated,
const okvis::VioParameters params,
const std::shared_ptr< okvis::MapPointVector map,
std::shared_ptr< okvis::MultiFrame framesInOut,
bool *  asKeyframe 
)
virtual

Matching as well as initialization of landmarks and state.

Warning
This method is not threadsafe.
This method uses the estimator. Make sure to not access it in another thread.
Parameters
estimatorEstimator.
T_WS_propagatedPose of sensor at image capture time.
paramsConfiguration parameters.
mapUnused.
framesInOutMultiframe including the descriptors of all the keypoints.
[out]asKeyframeShould the frame be a keyframe?
Returns
True if successful.

Implements okvis::VioFrontendInterface.

bool okvis::Frontend::detectAndDescribe ( size_t  cameraIndex,
std::shared_ptr< okvis::MultiFrame frameOut,
const okvis::kinematics::Transformation T_WC,
const std::vector< cv::KeyPoint > *  keypoints 
)
virtual

Detection and descriptor extraction on a per image basis.

Remarks
This method is threadsafe.
Parameters
cameraIndexIndex of camera to do detection and description.
frameOutMultiframe containing the frames. Resulting keypoints and descriptors are saved in here.
T_WCPose of camera with index cameraIndex at image capture time.
[in]keypointsIf the keypoints are already available from a different source, provide them here in order to skip detection.
Warning
Using keypoints from a different source is not yet implemented.
Returns
True if successful.

Implements okvis::VioFrontendInterface.

bool okvis::Frontend::doWeNeedANewKeyframe ( const okvis::Estimator estimator,
std::shared_ptr< okvis::MultiFrame currentFrame 
)
private

Decision whether a new frame should be keyframe or not.

Parameters
estimatorconst reference to the estimator.
currentFrameKeyframe candidate.
Returns
True if it should be a new keyframe.
bool okvis::Frontend::getBriskDescriptionRotationInvariance ( ) const
inline

Get the rotation invariance setting of the BRISK descriptor.

bool okvis::Frontend::getBriskDescriptionScaleInvariance ( ) const
inline

Get the scale invariance setting of the BRISK descriptor.

double okvis::Frontend::getBriskDetectionAbsoluteThreshold ( ) const
inline

Get the absolute threshold of the BRISK detector.

size_t okvis::Frontend::getBriskDetectionMaximumKeypoints ( ) const
inline

Get the maximum amount of keypoints of the BRISK detector.

size_t okvis::Frontend::getBriskDetectionOctaves ( ) const
inline

Get the number of octaves of the BRISK detector.

double okvis::Frontend::getBriskDetectionThreshold ( ) const
inline

Get the detection threshold of the BRISK detector.

double okvis::Frontend::getBriskMatchingThreshold ( ) const
inline

Get the matching threshold.

float okvis::Frontend::getKeyframeInsertionMatchingRatioThreshold ( ) const
inline

Get the matching ratio threshold under which a new keyframe is inserted.

float okvis::Frontend::getKeyframeInsertionOverlapThershold ( ) const
inline

Get the area overlap threshold under which a new keyframe is inserted.

void okvis::Frontend::initialiseBriskFeatureDetectors ( )
private

(re)instantiates feature detectors and descriptor extractors. Used after settings changed or at startup.

bool okvis::Frontend::isInitialized ( )
inline

Returns true if the initialization has been completed (RANSAC with actual translation)

template<class MATCHING_ALGORITHM >
void okvis::Frontend::matchStereo ( okvis::Estimator estimator,
std::shared_ptr< okvis::MultiFrame multiFrame 
)
private

Match the frames inside the multiframe to each other to initialise new landmarks.

Template Parameters
MATCHING_ALGORITHMAlgorithm to match new keypoints to existing landmarks.
Warning
As this function uses the estimator it is not threadsafe.
Parameters
estimatorEstimator.
multiFrameMultiframe containing the frames to match.
template<class MATCHING_ALGORITHM >
int okvis::Frontend::matchToKeyframes ( okvis::Estimator estimator,
const okvis::VioParameters params,
const uint64_t  currentFrameId,
bool &  rotationOnly,
bool  usePoseUncertainty = true,
double *  uncertainMatchFraction = 0,
bool  removeOutliers = true 
)
private

Match a new multiframe to existing keyframes.

Template Parameters
MATCHING_ALGORITHMAlgorithm to match new keypoints to existing landmarks
Warning
As this function uses the estimator it is not threadsafe
Parameters
estimatorEstimator.
[in]paramsParameter struct.
[in]currentFrameIdID of the current frame that should be matched against keyframes.
[out]rotationOnlyWas the rotation only RANSAC motion model good enough to explain the motion between the new frame and the keyframes?
[in]usePoseUncertaintyUse the pose uncertainty for the matching.
[out]uncertainMatchFractionReturn the fraction of uncertain matches. Set to nullptr if not interested.
[in]removeOutliersRemove outliers during RANSAC.
Returns
The number of matches in total.
template<class MATCHING_ALGORITHM >
int okvis::Frontend::matchToLastFrame ( okvis::Estimator estimator,
const okvis::VioParameters params,
const uint64_t  currentFrameId,
bool  usePoseUncertainty = true,
bool  removeOutliers = true 
)
private

Match a new multiframe to the last frame.

Template Parameters
MATCHING_ALGORITHMAlgorithm to match new keypoints to existing landmarks
Warning
As this function uses the estimator it is not threadsafe.
Parameters
estimatorEstimator.
paramsParameter struct.
currentFrameIdID of the current frame that should be matched against the last one.
usePoseUncertaintyUse the pose uncertainty for the matching.
removeOutliersRemove outliers during RANSAC.
Returns
The number of matches in total.
bool okvis::Frontend::propagation ( const okvis::ImuMeasurementDeque imuMeasurements,
const okvis::ImuParameters imuParams,
okvis::kinematics::Transformation T_WS_propagated,
okvis::SpeedAndBias speedAndBiases,
const okvis::Time t_start,
const okvis::Time t_end,
Eigen::Matrix< double, 15, 15 > *  covariance,
Eigen::Matrix< double, 15, 15 > *  jacobian 
) const
virtual

Propagates pose, speeds and biases with given IMU measurements.

See Also
okvis::ceres::ImuError::propagation()
Remarks
This method is threadsafe.
Parameters
[in]imuMeasurementsAll the IMU measurements.
[in]imuParamsThe parameters to be used.
[in,out]T_WS_propagatedStart pose.
[in,out]speedAndBiasesStart speed and biases.
[in]t_startStart time.
[in]t_endEnd time.
[out]covarianceCovariance for GIVEN start states.
[out]jacobianJacobian w.r.t. start states.
Returns
True on success.

Implements okvis::VioFrontendInterface.

int okvis::Frontend::runRansac2d2d ( okvis::Estimator estimator,
const okvis::VioParameters params,
uint64_t  currentFrameId,
uint64_t  olderFrameId,
bool  initializePose,
bool  removeOutliers,
bool &  rotationOnly 
)
private

Perform 2D/2D RANSAC.

Warning
As this function uses the estimator it is not threadsafe.
Parameters
estimatorEstimator.
paramsParameter struct.
currentFrameIdID of the new multiframe containing matches with the frame with ID olderFrameId.
olderFrameIdID of the multiframe to which the current frame has been matched against.
initializePoseIf the pose has not yet been initialised should the function try to initialise it.
removeOutliersRemove observation of outliers in estimator.
[out]rotationOnlyWas the rotation only RANSAC model enough to explain the matches.
Returns
Number of inliers.
int okvis::Frontend::runRansac3d2d ( okvis::Estimator estimator,
const okvis::cameras::NCameraSystem nCameraSystem,
std::shared_ptr< okvis::MultiFrame currentFrame,
bool  removeOutliers 
)
private

Perform 3D/2D RANSAC.

Warning
As this function uses the estimator it is not threadsafe.
Parameters
estimatorEstimator.
nCameraSystemCamera configuration and parameters.
currentFrameFrame with the new potential matches.
removeOutliersRemove observation of outliers in estimator.
Returns
Number of inliers.
void okvis::Frontend::setBriskDescriptionRotationInvariance ( bool  invariance)
inline

Set the rotation invariance setting of the BRISK descriptor.

void okvis::Frontend::setBriskDescriptionScaleInvariance ( bool  invariance)
inline

Set the scale invariance setting of the BRISK descriptor.

void okvis::Frontend::setBriskDetectionAbsoluteThreshold ( double  threshold)
inline

Set the absolute threshold of the BRISK detector.

void okvis::Frontend::setBriskDetectionMaximumKeypoints ( size_t  maxKeypoints)
inline

Set the maximum number of keypoints of the BRISK detector.

void okvis::Frontend::setBriskDetectionOctaves ( size_t  octaves)
inline

Set the number of octaves of the BRISK detector.

void okvis::Frontend::setBriskDetectionThreshold ( double  threshold)
inline

Set the detection threshold of the BRISK detector.

void okvis::Frontend::setBriskMatchingThreshold ( double  threshold)
inline

Set the matching threshold.

void okvis::Frontend::setKeyframeInsertionMatchingRatioThreshold ( float  threshold)
inline

Set the matching ratio threshold under which a new keyframe is inserted.

void okvis::Frontend::setKeyframeInsertionOverlapThreshold ( float  threshold)
inline

Set the area overlap threshold under which a new keyframe is inserted.

Member Data Documentation

bool okvis::Frontend::briskDescriptionRotationInvariance_
private

The set rotation invariance setting.

bool okvis::Frontend::briskDescriptionScaleInvariance_
private

The set scale invariance setting.

double okvis::Frontend::briskDetectionAbsoluteThreshold_
private

The set BRISK absolute detection threshold.

size_t okvis::Frontend::briskDetectionMaximumKeypoints_
private

The set maximum number of keypoints.

size_t okvis::Frontend::briskDetectionOctaves_
private

The set number of brisk octaves.

double okvis::Frontend::briskDetectionThreshold_
private

The set BRISK detection threshold.

double okvis::Frontend::briskMatchingThreshold_
private

The set BRISK matching threshold.

std::vector<std::shared_ptr<cv::DescriptorExtractor> > okvis::Frontend::descriptorExtractors_
private

feature descriptors with the current settings. The vector contains one for each camera to ensure that there are no problems with parallel detection.

Warning
Lock with featureDetectorMutexes_[cameraIndex] when using the descriptor.
std::vector<std::unique_ptr<std::mutex> > okvis::Frontend::featureDetectorMutexes_
private

Mutexes for feature detectors and descriptors.

std::vector<std::shared_ptr<cv::FeatureDetector> > okvis::Frontend::featureDetectors_
private

feature detectors with the current settings. The vector contains one for each camera to ensure that there are no problems with parallel detection.

Warning
Lock with featureDetectorMutexes_[cameraIndex] when using the detector.
bool okvis::Frontend::isInitialized_
private

Is the pose initialised?

float okvis::Frontend::keyframeInsertionMatchingRatioThreshold_
private

If the number of matched keypoints of the current frame with an older frame divided by the amount of points inside the convex hull around all keypoints is lower than the threshold it should be a keyframe.

See Also
doWeNeedANewKeyframe()
float okvis::Frontend::keyframeInsertionOverlapThreshold_
private

If the hull-area around all matched keypoints of the current frame (with existing landmarks) divided by the hull-area around all keypoints in the current frame is lower than this threshold it should be a new keyframe.

See Also
doWeNeedANewKeyframe()
std::unique_ptr<okvis::DenseMatcher> okvis::Frontend::matcher_
private

Matcher object.

const size_t okvis::Frontend::numCameras_
private

Number of cameras in the configuration.


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