This class manages the complete data flow in and out of the algorithm, as well as between the processing threads. More...

#include <ThreadedKFVio.hpp>

Inheritance diagram for okvis::ThreadedKFVio:

Classes
struct	OptimizationResults
	This struct contains the results of the optimization for ease of publication. It is also used for publishing poses that have been propagated with the IMU measurements. More...

Public Member Functions
	ThreadedKFVio (okvis::VioParameters &parameters)
	Constructor. More...

virtual	~ThreadedKFVio ()
	Destructor. This calls Shutdown() for all threadsafe queues and joins all threads. More...

void	display ()
	Trigger display (needed because OSX won't allow threaded display). More...

Add measurements to the algorithm
virtual bool	addImage (const okvis::Time &stamp, size_t cameraIndex, const cv::Mat &image, const std::vector< cv::KeyPoint > keypoints=0, bool asKeyframe=0)
	Add a new image. More...

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)
	Add an abstracted image observation. More...

virtual bool	addImuMeasurement (const okvis::Time &stamp, const Eigen::Vector3d &alpha, const Eigen::Vector3d &omega)
	Add an IMU measurement. More...

virtual void	addPositionMeasurement (const okvis::Time &stamp, const Eigen::Vector3d &position, const Eigen::Vector3d &positionOffset, const Eigen::Matrix3d &positionCovariance)
	Add a position measurement. More...

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)
	Add a GPS measurement. More...

virtual void	addMagnetometerMeasurement (const okvis::Time &stamp, const Eigen::Vector3d &fluxDensityMeas, double stdev)
	Add a magnetometer measurement. More...

virtual void	addBarometerMeasurement (const okvis::Time &stamp, double staticPressure, double stdev)
	Add a static pressure measurement. More...

virtual void	addDifferentialPressureMeasurement (const okvis::Time &stamp, double differentialPressure, double stdev)
	Add a differential pressure measurement. More...

Setters
virtual void	setBlocking (bool blocking)
	Set the blocking variable that indicates whether the addMeasurement() functions should return immediately (blocking=false), or only when the processing is complete. More...

Public Member Functions inherited from okvis::VioInterface
	VioInterface ()

virtual	~VioInterface ()

bool	setImuCsvFile (std::fstream &csvFile)
	Set a CVS file where the IMU data will be saved to. More...

bool	setImuCsvFile (const std::string &csvFileName)
	Set a CVS file where the IMU data will be saved to. More...

bool	setTracksCsvFile (size_t cameraId, std::fstream &csvFile)
	Set a CVS file where the tracks (data associations) will be saved to. More...

bool	setTracksCsvFile (size_t cameraId, const std::string &csvFileName)
	Set a CVS file where the tracks (data associations) will be saved to. More...

bool	setPosCsvFile (std::fstream &csvFile)
	Set a CVS file where the position measurements will be saved to. More...

bool	setPosCsvFile (const std::string &csvFileName)
	Set a CVS file where the position measurements will be saved to. More...

bool	setMagCsvFile (std::fstream &csvFile)
	Set a CVS file where the magnetometer measurements will be saved to. More...

bool	setMagCsvFile (const std::string &csvFileName)
	Set a CVS file where the magnetometer measurements will be saved to. More...

virtual void	setStateCallback (const StateCallback &stateCallback)
	Set the stateCallback to be called every time a new state is estimated. When an implementing class has an estimate, they can call: stateCallback_( stamp, T_w_vk ); 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. More...

virtual void	setFullStateCallback (const FullStateCallback &fullStateCallback)
	Set the fullStateCallback to be called every time a new state is estimated. When an implementing class has an estimate, they can call: _fullStateCallback( stamp, T_w_vk, speedAndBiases, omega_S); 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. finally, omega_S is the rotation speed. More...

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. More...

virtual void	setLandmarksCallback (const LandmarksCallback &landmarksCallback)
	Set the landmarksCallback to be called every time a new state is estimated. When an implementing class has an estimate, they can call: landmarksCallback_( stamp, landmarksVector ); where stamp is the timestamp landmarksVector contains all 3D-landmarks with id. More...

bool	addEigenImage (const okvis::Time &stamp, size_t cameraIndex, const EigenImage &image)
	This is just handy for the python interface. More...

Public Attributes
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef okvis::timing::Timer	TimerSwitchable

Private Member Functions
virtual void	startThreads ()
	Start all threads. More...

void	init ()
	Initialises settings and calls startThreads(). More...

void	frameConsumerLoop (size_t cameraIndex)
	Loop to process frames from camera with index cameraIndex. More...

void	matchingLoop ()
	Loop that matches frames with existing frames. More...

void	imuConsumerLoop ()
	Loop to process IMU measurements. More...

void	positionConsumerLoop ()
	Loop to process position measurements. More...

void	gpsConsumerLoop ()
	Loop to process GPS measurements. More...

void	magnetometerConsumerLoop ()
	Loop to process magnetometer measurements. More...

void	differentialConsumerLoop ()
	Loop to process differential pressure measurements. More...

void	visualizationLoop ()
	Loop that visualizes completed frames. More...

void	optimizationLoop ()
	Loop that performs the optimization and marginalisation. More...

void	publisherLoop ()
	Loop that publishes the newest state and landmarks. More...

okvis::ImuMeasurementDeque	getImuMeasurments (okvis::Time &start, okvis::Time &end)
	Get a subset of the recorded IMU measurements. More...

int	deleteImuMeasurements (const okvis::Time &eraseUntil)
	Remove IMU measurements from the internal buffer. More...

Private Attributes
ImuFrameSynchronizer	imuFrameSynchronizer_

okvis::FrameSynchronizer	frameSynchronizer_
	The frame synchronizer responsible for merging frames into multiframes. More...

okvis::Time	lastAddedStateTimestamp_
	Timestamp of the newest state in the Estimator. More...

okvis::Time	lastAddedImageTimestamp_
	Timestamp of the newest image added to the image input queue. More...

size_t	numCameras_
	Number of cameras in the system. More...

size_t	numCameraPairs_
	Number of camera pairs in the system. More...

okvis::VioParameters	parameters_
	The parameters and settings. More...

const size_t	maxImuInputQueueSize_

const size_t	maxPositionInputQueueSize_ = 10
	Max position measurements before dropping. More...

State variables
okvis::SpeedAndBias	speedAndBiases_propagated_

okvis::ImuParameters	imu_params_
	The IMU parameters. More...

okvis::kinematics::Transformation	T_WS_propagated_
	The pose propagated by the IMU measurements. More...

std::shared_ptr < okvis::MapPointVector >	map_
	The map. Unused. More...

okvis::kinematics::Transformation	lastOptimized_T_WS_
	Resulting pose of the last optimization. More...

okvis::SpeedAndBias	lastOptimizedSpeedAndBiases_
	Resulting speeds and IMU biases after last optimization. More...

okvis::Time	lastOptimizedStateTimestamp_
	Timestamp of newest frame used in the last optimization. More...

std::atomic_bool	repropagationNeeded_

Measurement input queues
std::vector< std::shared_ptr < okvis::threadsafe::ThreadSafeQueue < std::shared_ptr < okvis::CameraMeasurement > > > >	cameraMeasurementsReceived_
	Camera measurement input queues. For each camera in the configuration one. More...

okvis::threadsafe::ThreadSafeQueue < okvis::ImuMeasurement >	imuMeasurementsReceived_
	IMU measurement input queue. More...

okvis::threadsafe::ThreadSafeQueue < okvis::PositionMeasurement >	positionMeasurementsReceived_
	Position measurement input queue. More...

Measurement operation queues.
okvis::threadsafe::ThreadSafeQueue < std::shared_ptr < okvis::MultiFrame > >	keypointMeasurements_
	The queue containing multiframes with completely detected frames. More...

okvis::threadsafe::ThreadSafeQueue < std::shared_ptr < okvis::MultiFrame > >	matchedFrames_
	The queue containing multiframes with completely matched frames. These are already part of the estimator state. More...

okvis::ImuMeasurementDeque	imuMeasurements_
	The IMU measurements. More...

okvis::PositionMeasurementDeque	positionMeasurements_
	The Position measurements. More...

okvis::threadsafe::ThreadSafeQueue < OptimizationResults >	optimizationResults_
	The queue containing the results of the optimization or IMU propagation ready for publishing. More...

okvis::threadsafe::ThreadSafeQueue < VioVisualizer::VisualizationData::Ptr >	visualizationData_
	The queue containing visualization data that is ready to be displayed. More...

okvis::threadsafe::ThreadSafeQueue < std::vector< cv::Mat > >	displayImages_
	The queue containing the actual display images. More...

Mutexes
std::mutex	imuMeasurements_mutex_
	Lock when accessing imuMeasurements_. More...

std::mutex	positionMeasurements_mutex_
	Lock when accessing imuMeasurements_. More...

std::mutex	frameSynchronizer_mutex_
	Lock when accessing the frameSynchronizer_. More...

std::mutex	estimator_mutex_
	Condition variable to signalise that optimization is done. More...

std::condition_variable	optimizationNotification_

std::atomic_bool	optimizationDone_
	Boolean flag for whether optimization is done for the last state that has been added to the estimator. More...

std::mutex	lastState_mutex_
	Lock when accessing any of the 'lastOptimized*' variables. More...


std::vector< std::thread >	frameConsumerThreads_
	Threads running the frameConsumerLoop(). One per camera. More...

std::vector< std::thread >	keypointConsumerThreads_
	Threads running matchingLoop(). More...

std::vector< std::thread >	matchesConsumerThreads_
	Unused. More...

std::thread	imuConsumerThread_
	Thread running imuConsumerLoop(). More...

std::thread	positionConsumerThread_
	Thread running positionConsumerLoop(). More...

std::thread	gpsConsumerThread_
	Thread running gpsConsumerLoop(). More...

std::thread	magnetometerConsumerThread_
	Thread running magnetometerConsumerLoop(). More...

std::thread	differentialConsumerThread_
	Thread running differentialConsumerLoop(). More...

Algorithm threads
std::thread	visualizationThread_
	Thread running visualizationLoop(). More...

std::thread	optimizationThread_
	Thread running optimizationLoop(). More...

std::thread	publisherThread_
	Thread running publisherLoop(). More...

Algorithm objects.
okvis::Estimator	estimator_
	The backend estimator. More...

okvis::Frontend	frontend_
	The frontend. More...

Additional Inherited Members
Public Types inherited from okvis::VioInterface
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

Protected Types inherited from okvis::VioInterface
typedef std::map< size_t, std::shared_ptr< std::fstream > >	FilePtrMap

Protected Member Functions inherited from okvis::VioInterface
bool	writeImuCsvDescription ()
	Write first line of IMU CSV file to describe columns. More...

bool	writePosCsvDescription ()
	Write first line of position CSV file to describe columns. More...

bool	writeMagCsvDescription ()
	Write first line of magnetometer CSV file to describe columns. More...

bool	writeTracksCsvDescription (size_t cameraId)
	Write first line of tracks (data associations) CSV file to describe columns. More...

Protected Attributes inherited from okvis::VioInterface
StateCallback	stateCallback_
	State callback function. More...

FullStateCallback	fullStateCallback_
	Full state callback function. More...

FullStateCallbackWithExtrinsics	fullStateCallbackWithExtrinsics_
	Full state and extrinsics callback function. More...

LandmarksCallback	landmarksCallback_
	Landmarks callback function. More...

std::shared_ptr< std::fstream >	csvImuFile_
	IMU CSV file. More...

std::shared_ptr< std::fstream >	csvPosFile_
	Position CSV File. More...

std::shared_ptr< std::fstream >	csvMagFile_
	Magnetometer CSV File. More...

FilePtrMap	csvTracksFiles_
	Tracks CSV Files. More...

bool	blocking_
	Blocking option. Whether the addMeasurement() functions should wait until proccessing is complete. More...

Detailed Description

This class manages the complete data flow in and out of the algorithm, as well as between the processing threads.

To ensure fast return from user callbacks, new data are collected in thread save input queues and are processed in data consumer threads, one for each data source. The algorithm tasks are running in individual threads with thread save queue for message passing. For sending back data to the user, publisher threads are created for each output which ensure that the algorithm is not being blocked by slow users.

All the queues can be limited to size 1 to back propagate processing congestions to the user callback.

Constructor & Destructor Documentation

okvis::ThreadedKFVio::ThreadedKFVio ( okvis::VioParameters & parameters )

Constructor.

Parameters

parameters Parameters and settings.

okvis::ThreadedKFVio::~ThreadedKFVio ( )

virtual

Destructor. This calls Shutdown() for all threadsafe queues and joins all threads.

Member Function Documentation

void okvis::ThreadedKFVio::addBarometerMeasurement	(	const okvis::Time &	stamp,
		double	staticPressure,
		double	stdev
	)

virtual

Add a static pressure measurement.

Warning: Not implemented.

Parameters

stamp	The measurement timestamp.
staticPressure	Measured static pressure [Pa].
stdev	Measurement std deviation [Pa].

Reimplemented from okvis::VioInterface.

void okvis::ThreadedKFVio::addDifferentialPressureMeasurement	(	const okvis::Time &	stamp,
		double	differentialPressure,
		double	stdev
	)

virtual

Add a differential pressure measurement.

Warning: Not implemented.

Parameters

stamp	The measurement timestamp.
differentialPressure	Measured differential pressure [Pa].
stdev	Measurement std deviation [Pa].

Reimplemented from okvis::VioInterface.

void okvis::ThreadedKFVio::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
	)

virtual

Add a GPS measurement.

Warning: Not implemented.

Parameters

stamp	The measurement timestamp.
lat_wgs84_deg	WGS84 latitude [deg].
lon_wgs84_deg	WGS84 longitude [deg].
alt_wgs84_deg	WGS84 altitude [m].
positionOffset	Body frame antenna position offset [m].
positionCovarianceENU	The position measurement covariance matrix.

Reimplemented from okvis::VioInterface.

bool okvis::ThreadedKFVio::addImage	(	const okvis::Time &	stamp,
		size_t	cameraIndex,
		const cv::Mat &	image,
		const std::vector< cv::KeyPoint > *	keypoints = `0`,
		bool *	asKeyframe = `0`
	)

virtual

Add a new image.

Parameters

stamp	The image timestamp.
cameraIndex	The index of the camera that the image originates from.
image	The image.
keypoints	Optionally aready pass keypoints. This will skip the detection part.
asKeyframe	Use the new image as keyframe. Not implemented.

Warning: The frame consumer loop does not support using existing keypoints yet.; Already specifying whether this frame should be a keyframe is not implemented yet.

Returns: Returns true normally. False, if the previous one has not been processed yet.

Implements okvis::VioInterface.

bool okvis::ThreadedKFVio::addImuMeasurement	(	const okvis::Time &	stamp,
		const Eigen::Vector3d &	alpha,
		const Eigen::Vector3d &	omega
	)

virtual

Add an IMU measurement.

Parameters

stamp	The measurement timestamp.
alpha	The acceleration measured at this time.
omega	The angular velocity measured at this time.

Returns: Returns true normally. False if the previous one has not been processed yet.

Implements okvis::VioInterface.

bool okvis::ThreadedKFVio::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`
	)

virtual

Add an abstracted image observation.

Warning: Not implemented.

Parameters

stamp	The timestamp for the start of integration time for the image.
cameraIndex	The index of the camera.
keypoints	A vector where each entry represents a [u,v] keypoint measurement. Also set the size field.
landmarkIds	A vector of landmark ids for each keypoint measurement.
descriptors	A matrix containing the descriptors for each keypoint.
asKeyframe	Optionally force keyframe or not.

Returns: Returns true normally. False, if the previous one has not been processed yet.

Implements okvis::VioInterface.

void okvis::ThreadedKFVio::addMagnetometerMeasurement	(	const okvis::Time &	stamp,
		const Eigen::Vector3d &	fluxDensityMeas,
		double	stdev
	)

virtual

Add a magnetometer measurement.

Warning: Not implemented.

Parameters

stamp	The measurement timestamp.
fluxDensityMeas	Measured magnetic flux density (sensor frame) [uT].
stdev	Measurement std deviation [uT].

Reimplemented from okvis::VioInterface.

void okvis::ThreadedKFVio::addPositionMeasurement	(	const okvis::Time &	stamp,
		const Eigen::Vector3d &	position,
		const Eigen::Vector3d &	positionOffset,
		const Eigen::Matrix3d &	positionCovariance
	)

virtual

Add a position measurement.

Warning: Not implemented.

Parameters

stamp	The measurement timestamp.
position	The position in world frame.
positionOffset	Body frame antenna position offset [m].
positionCovariance	The position measurement covariance matrix.

Reimplemented from okvis::VioInterface.

int okvis::ThreadedKFVio::deleteImuMeasurements ( const okvis::Time & eraseUntil )

private

Remove IMU measurements from the internal buffer.

Parameters

eraseUntil Remove all measurements that are strictly older than this time.

Returns: The number of IMU measurements that have been removed

void okvis::ThreadedKFVio::differentialConsumerLoop ( )

private

Loop to process differential pressure measurements.

Warning: Not implemented.

void okvis::ThreadedKFVio::display ( )

Trigger display (needed because OSX won't allow threaded display).

void okvis::ThreadedKFVio::frameConsumerLoop ( size_t cameraIndex )

private

Loop to process frames from camera with index cameraIndex.

okvis::ImuMeasurementDeque okvis::ThreadedKFVio::getImuMeasurments	(	okvis::Time &	start,
		okvis::Time &	end
	)

private

Get a subset of the recorded IMU measurements.

Parameters

start	The first IMU measurement in the return value will be older than this timestamp.
end	The last IMU measurement in the return value will be newer than this timestamp.

Remarks: This function is threadsafe.

Returns: The IMU Measurement spanning at least the time between start and end.

void okvis::ThreadedKFVio::gpsConsumerLoop ( )

private

Loop to process GPS measurements.

Warning: Not implemented.

void okvis::ThreadedKFVio::imuConsumerLoop ( )

private

Loop to process IMU measurements.

void okvis::ThreadedKFVio::init ( )

private

Initialises settings and calls startThreads().

void okvis::ThreadedKFVio::magnetometerConsumerLoop ( )

private

Loop to process magnetometer measurements.

Warning: Not implemented.

void okvis::ThreadedKFVio::matchingLoop ( )

private

Loop that matches frames with existing frames.

void okvis::ThreadedKFVio::optimizationLoop ( )

private

Loop that performs the optimization and marginalisation.

void okvis::ThreadedKFVio::positionConsumerLoop ( )

private

Loop to process position measurements.

Warning: Not implemented.

void okvis::ThreadedKFVio::publisherLoop ( )

private

Loop that publishes the newest state and landmarks.

void okvis::ThreadedKFVio::setBlocking ( bool blocking )

virtual

Set the blocking variable that indicates whether the addMeasurement() functions should return immediately (blocking=false), or only when the processing is complete.

Reimplemented from okvis::VioInterface.

void okvis::ThreadedKFVio::startThreads ( )

privatevirtual

Start all threads.

void okvis::ThreadedKFVio::visualizationLoop ( )

private

Loop that visualizes completed frames.

Member Data Documentation

std::vector<std::shared_ptr< okvis::threadsafe::ThreadSafeQueue<std::shared_ptr<okvis::CameraMeasurement> > > > okvis::ThreadedKFVio::cameraMeasurementsReceived_

private

Camera measurement input queues. For each camera in the configuration one.

std::thread okvis::ThreadedKFVio::differentialConsumerThread_

private

Thread running differentialConsumerLoop().

okvis::threadsafe::ThreadSafeQueue<std::vector<cv::Mat> > okvis::ThreadedKFVio::displayImages_

private

The queue containing the actual display images.

okvis::Estimator okvis::ThreadedKFVio::estimator_

private

The backend estimator.

std::mutex okvis::ThreadedKFVio::estimator_mutex_

private

Condition variable to signalise that optimization is done.

Lock when accessing the estimator_.

std::vector<std::thread> okvis::ThreadedKFVio::frameConsumerThreads_

private

Threads running the frameConsumerLoop(). One per camera.

okvis::FrameSynchronizer okvis::ThreadedKFVio::frameSynchronizer_

private

The frame synchronizer responsible for merging frames into multiframes.

Warning: Lock with frameSynchronizer_mutex_

std::mutex okvis::ThreadedKFVio::frameSynchronizer_mutex_

private

Lock when accessing the frameSynchronizer_.

okvis::Frontend okvis::ThreadedKFVio::frontend_

private

The frontend.

std::thread okvis::ThreadedKFVio::gpsConsumerThread_

private

Thread running gpsConsumerLoop().

okvis::ImuParameters okvis::ThreadedKFVio::imu_params_

private

The IMU parameters.

Warning: Duplicate of parameters_.imu

std::thread okvis::ThreadedKFVio::imuConsumerThread_

private

Thread running imuConsumerLoop().

ImuFrameSynchronizer okvis::ThreadedKFVio::imuFrameSynchronizer_

private

The IMU frame synchronizer.

okvis::ImuMeasurementDeque okvis::ThreadedKFVio::imuMeasurements_

private

The IMU measurements.

Warning: Lock with imuMeasurements_mutex_.

std::mutex okvis::ThreadedKFVio::imuMeasurements_mutex_

private

Lock when accessing imuMeasurements_.

okvis::threadsafe::ThreadSafeQueue<okvis::ImuMeasurement> okvis::ThreadedKFVio::imuMeasurementsReceived_

private

IMU measurement input queue.

std::vector<std::thread> okvis::ThreadedKFVio::keypointConsumerThreads_

private

Threads running matchingLoop().

okvis::threadsafe::ThreadSafeQueue<std::shared_ptr<okvis::MultiFrame> > okvis::ThreadedKFVio::keypointMeasurements_

private

The queue containing multiframes with completely detected frames.

okvis::Time okvis::ThreadedKFVio::lastAddedImageTimestamp_

private

Timestamp of the newest image added to the image input queue.

okvis::Time okvis::ThreadedKFVio::lastAddedStateTimestamp_

private

Timestamp of the newest state in the Estimator.

okvis::kinematics::Transformation okvis::ThreadedKFVio::lastOptimized_T_WS_

private

Resulting pose of the last optimization.

Warning: Lock lastState_mutex_.

okvis::SpeedAndBias okvis::ThreadedKFVio::lastOptimizedSpeedAndBiases_

private

Resulting speeds and IMU biases after last optimization.

Warning: Lock lastState_mutex_.

okvis::Time okvis::ThreadedKFVio::lastOptimizedStateTimestamp_

private

Timestamp of newest frame used in the last optimization.

Warning: Lock lastState_mutex_.

std::mutex okvis::ThreadedKFVio::lastState_mutex_

private

Lock when accessing any of the 'lastOptimized*' variables.

std::thread okvis::ThreadedKFVio::magnetometerConsumerThread_

private

Thread running magnetometerConsumerLoop().

std::shared_ptr<okvis::MapPointVector> okvis::ThreadedKFVio::map_

private

The map. Unused.

okvis::threadsafe::ThreadSafeQueue<std::shared_ptr<okvis::MultiFrame> > okvis::ThreadedKFVio::matchedFrames_

private

The queue containing multiframes with completely matched frames. These are already part of the estimator state.

std::vector<std::thread> okvis::ThreadedKFVio::matchesConsumerThreads_

private

Unused.

const size_t okvis::ThreadedKFVio::maxImuInputQueueSize_

private

The maximum input queue size before IMU measurements are dropped. The maximum input queue size for the camera measurements is proportionally higher depending on the ratio between IMU and camera rate.

const size_t okvis::ThreadedKFVio::maxPositionInputQueueSize_ = 10

private

Max position measurements before dropping.

size_t okvis::ThreadedKFVio::numCameraPairs_

private

Number of camera pairs in the system.

size_t okvis::ThreadedKFVio::numCameras_

private

Number of cameras in the system.

std::atomic_bool okvis::ThreadedKFVio::optimizationDone_

private

Boolean flag for whether optimization is done for the last state that has been added to the estimator.

std::condition_variable okvis::ThreadedKFVio::optimizationNotification_

private

okvis::threadsafe::ThreadSafeQueue<OptimizationResults> okvis::ThreadedKFVio::optimizationResults_

private

The queue containing the results of the optimization or IMU propagation ready for publishing.

std::thread okvis::ThreadedKFVio::optimizationThread_

private

Thread running optimizationLoop().

okvis::VioParameters okvis::ThreadedKFVio::parameters_

private

The parameters and settings.

std::thread okvis::ThreadedKFVio::positionConsumerThread_

private

Thread running positionConsumerLoop().

okvis::PositionMeasurementDeque okvis::ThreadedKFVio::positionMeasurements_

private

The Position measurements.

Warning: Lock with positionMeasurements_mutex_.

std::mutex okvis::ThreadedKFVio::positionMeasurements_mutex_

private

Lock when accessing imuMeasurements_.

okvis::threadsafe::ThreadSafeQueue<okvis::PositionMeasurement> okvis::ThreadedKFVio::positionMeasurementsReceived_

private

Position measurement input queue.

std::thread okvis::ThreadedKFVio::publisherThread_

private

Thread running publisherLoop().

std::atomic_bool okvis::ThreadedKFVio::repropagationNeeded_

private

This is set to true after optimization to signal the IMU consumer loop to repropagate the state from the lastOptimizedStateTimestamp_.

okvis::SpeedAndBias okvis::ThreadedKFVio::speedAndBiases_propagated_

private

The speeds and IMU biases propagated by the IMU measurements.

okvis::kinematics::Transformation okvis::ThreadedKFVio::T_WS_propagated_

private

The pose propagated by the IMU measurements.

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef okvis::timing::Timer okvis::ThreadedKFVio::TimerSwitchable

okvis::threadsafe::ThreadSafeQueue<VioVisualizer::VisualizationData::Ptr> okvis::ThreadedKFVio::visualizationData_

private

The queue containing visualization data that is ready to be displayed.

std::thread okvis::ThreadedKFVio::visualizationThread_

private

Thread running visualizationLoop().

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

okvis/okvis_multisensor_processing/include/okvis/ThreadedKFVio.hpp
okvis/okvis_multisensor_processing/src/ThreadedKFVio.cpp

Classes

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation