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

The estimator class. More...

#include <Estimator.hpp>

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

Classes

struct  StateInfo
 StateInfo This configures the state vector ordering. More...
 
struct  States
 States This summarizes all the possible states – i.e. their ids: More...
 

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW Estimator ()
 The default constructor. More...
 
 Estimator (std::shared_ptr< okvis::ceres::Map > mapPtr)
 Constructor if a ceres map is already available. More...
 
virtual ~Estimator ()
 
bool addStates (okvis::MultiFramePtr multiFrame, const okvis::ImuMeasurementDeque &imuMeasurements, bool asKeyframe)
 Add a pose to the state. More...
 
void printStates (uint64_t poseId, std::ostream &buffer) const
 Prints state information to buffer. More...
 
bool addLandmark (uint64_t landmarkId, const Eigen::Vector4d &landmark)
 Add a landmark. More...
 
template<class GEOMETRY_TYPE >
::ceres::ResidualBlockId addObservation (uint64_t landmarkId, uint64_t poseId, size_t camIdx, size_t keypointIdx)
 Add an observation to a landmark. More...
 
bool removeObservation (uint64_t landmarkId, uint64_t poseId, size_t camIdx, size_t keypointIdx)
 Remove an observation from a landmark, if available. More...
 
bool applyMarginalizationStrategy (size_t numKeyframes, size_t numImuFrames, okvis::MapPointVector &removedLandmarks)
 Applies the dropping/marginalization strategy according to the RSS'13/IJRR'14 paper. The new number of frames in the window will be numKeyframes+numImuFrames. More...
 
void optimize (size_t numIter, size_t numThreads=1, bool verbose=false)
 Start ceres optimization. More...
 
bool setOptimizationTimeLimit (double timeLimit, int minIterations)
 Set a time limit for the optimization process. More...
 
bool isLandmarkAdded (uint64_t landmarkId) const
 Checks whether the landmark is added to the estimator. More...
 
bool isLandmarkInitialized (uint64_t landmarkId) const
 Checks whether the landmark is initialized. More...
 
bool isKeyframe (uint64_t frameId) const
 Checks if a particular frame is a keyframe. More...
 
bool isInImuWindow (uint64_t frameId) const
 Checks if a particular frame is still in the IMU window. More...
 
template<class GEOMETRY_TYPE >
::ceres::ResidualBlockId addObservation (uint64_t landmarkId, uint64_t poseId, size_t camIdx, size_t keypointIdx)
 
Sensor configuration related
int addCamera (const okvis::ExtrinsicsEstimationParameters &extrinsicsEstimationParameters)
 Add a camera to the configuration. Sensors can only be added and never removed. More...
 
int addImu (const okvis::ImuParameters &imuParameters)
 Add an IMU to the configuration. More...
 
void clearCameras ()
 Remove all cameras from the configuration. More...
 
void clearImus ()
 Remove all IMUs from the configuration. More...
 
Getters
bool getLandmark (uint64_t landmarkId, okvis::MapPoint &mapPoint) const
 Get a specific landmark. More...
 
size_t getLandmarks (okvis::PointMap &landmarks) const
 Get a copy of all the landmarks as a PointMap. More...
 
size_t getLandmarks (okvis::MapPointVector &landmarks) const
 Get a copy of all the landmark in a MapPointVector. This is for legacy support. Use getLandmarks(okvis::PointMap&) if possible. More...
 
okvis::MultiFramePtr multiFrame (uint64_t frameId) const
 Get a multiframe. More...
 
bool get_T_WS (uint64_t poseId, okvis::kinematics::Transformation &T_WS) const
 Get pose for a given pose ID. More...
 
bool getSpeedAndBias (uint64_t poseId, uint64_t imuIdx, okvis::SpeedAndBias &speedAndBias) const
 Get speeds and IMU biases for a given pose ID. More...
 
bool getCameraSensorStates (uint64_t poseId, size_t cameraIdx, okvis::kinematics::Transformation &T_SCi) const
 Get camera states for a given pose ID. More...
 
size_t numFrames () const
 Get the number of states/frames in the estimator. More...
 
size_t numLandmarks () const
 Get the number of landmarks in the estimator. More...
 
uint64_t currentKeyframeId () const
 Get the ID of the current keyframe. More...
 
uint64_t frameIdByAge (size_t age) const
 Get the ID of an older frame. More...
 
uint64_t currentFrameId () const
 Get the ID of the newest frame added to the state. More...
 
okvis::Time timestamp (uint64_t frameId) const
 Get the timestamp for a particular frame. More...
 
Setters
bool set_T_WS (uint64_t poseId, const okvis::kinematics::Transformation &T_WS)
 Set pose for a given pose ID. More...
 
bool setSpeedAndBias (uint64_t poseId, size_t imuIdx, const okvis::SpeedAndBias &speedAndBias)
 Set the speeds and IMU biases for a given pose ID. More...
 
bool setCameraSensorStates (uint64_t poseId, size_t cameraIdx, const okvis::kinematics::Transformation &T_SCi)
 Set the transformation from sensor to camera frame for a given pose ID. More...
 
bool setLandmark (uint64_t landmarkId, const Eigen::Vector4d &landmark)
 Set the homogeneous coordinates for a landmark. More...
 
void setLandmarkInitialized (uint64_t landmarkId, bool initialized)
 Set the landmark initialization state. More...
 
void setKeyframe (uint64_t frameId, bool isKeyframe)
 Set whether a frame is a keyframe or not. More...
 
void setMap (std::shared_ptr< okvis::ceres::Map > mapPtr)
 set ceres map More...
 
- Public Member Functions inherited from okvis::VioBackendInterface
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VioBackendInterface ()
 Default constructor. More...
 
virtual ~VioBackendInterface ()
 
virtual InitializationStatus initializationStatus () const
 Obtain the initialization status. 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...
 
virtual void setComputeUncertainty (bool)
 Request computation of uncertainties. param[in] computeUncertainty True, if uncertainties should be computed. More...
 

Static Public Member Functions

static bool initPoseFromImu (const okvis::ImuMeasurementDeque &imuMeasurements, okvis::kinematics::Transformation &T_WS)
 Initialise pose from IMU measurements. For convenience as static. More...
 

Private Types

enum  GlobalStates { T_WS = 0, MagneticZBias = 1, Qff = 2, T_GW = 3 }
 GlobalStates The global states enumerated. More...
 
enum  SensorStates {
  Camera = 0, Imu = 1, Position = 2, Gps = 3,
  Magnetometer = 4, StaticPressure = 5, DynamicPressure = 6
}
 SensorStates The sensor-internal states enumerated. More...
 
enum  CameraSensorStates { T_SCi = 0, Intrinsics = 1 }
 CameraSensorStates The camera-internal states enumerated. More...
 
enum  ImuSensorStates { SpeedAndBias = 0 }
 ImuSensorStates The IMU-internal states enumerated. More...
 
enum  PositionSensorStates { T_PiW = 0, PositionSensorB_t_BA = 1 }
 PositionSensorStates, currently unused. More...
 
enum  GpsSensorStates { GpsB_t_BA = 0 }
 GpsSensorStates, currently unused. More...
 
enum  MagnetometerSensorStates { MagnetometerBias = 0 }
 MagnetometerSensorStates, currently unused. More...
 
enum  StaticPressureSensorStates { StaticPressureBias = 0 }
 GpsSensorStates, currently unused. More...
 
enum  DynamicPressureSensorStates { DynamicPressureBias = 0 }
 GpsSensorStates, currently unused. More...
 
typedef std::array< StateInfo, 6 > GlobalStatesContainer
 Container for global states. More...
 
typedef std::vector< StateInfoSpecificSensorStatesContainer
 Container for sensor states. The dimension can vary from sensor to sensor... More...
 
typedef std::array
< std::vector
< SpecificSensorStatesContainer >, 7 > 
AllSensorStatesContainer
 Union of all sensor states. More...
 

Private Member Functions

bool removeObservation (::ceres::ResidualBlockId residualBlockId)
 Remove an observation from a landmark. More...
 
bool getGlobalStateParameterBlockPtr (uint64_t poseId, int stateType, std::shared_ptr< ceres::ParameterBlock > &stateParameterBlockPtr) const
 
template<class PARAMETER_BLOCK_T >
bool getGlobalStateParameterBlockAs (uint64_t poseId, int stateType, PARAMETER_BLOCK_T &stateParameterBlock) const
 
template<class PARAMETER_BLOCK_T >
bool getGlobalStateEstimateAs (uint64_t poseId, int stateType, typename PARAMETER_BLOCK_T::estimate_t &state) const
 
bool getSensorStateParameterBlockPtr (uint64_t poseId, int sensorIdx, int sensorType, int stateType, std::shared_ptr< ceres::ParameterBlock > &stateParameterBlockPtr) const
 
template<class PARAMETER_BLOCK_T >
bool getSensorStateParameterBlockAs (uint64_t poseId, int sensorIdx, int sensorType, int stateType, PARAMETER_BLOCK_T &stateParameterBlock) const
 
template<class PARAMETER_BLOCK_T >
bool getSensorStateEstimateAs (uint64_t poseId, int sensorIdx, int sensorType, int stateType, typename PARAMETER_BLOCK_T::estimate_t &state) const
 
template<class PARAMETER_BLOCK_T >
bool setGlobalStateEstimateAs (uint64_t poseId, int stateType, const typename PARAMETER_BLOCK_T::estimate_t &state)
 
template<class PARAMETER_BLOCK_T >
bool setSensorStateEstimateAs (uint64_t poseId, int sensorIdx, int sensorType, int stateType, const typename PARAMETER_BLOCK_T::estimate_t &state)
 

Private Attributes

std::map< uint64_t, StatesstatesMap_
 Buffer for currently considered states. More...
 
std::map< uint64_t,
okvis::MultiFramePtr
multiFramePtrMap_
 remember all needed okvis::MultiFrame. More...
 
std::shared_ptr
< okvis::ceres::Map
mapPtr_
 The underlying okvis::Map. More...
 
uint64_t referencePoseId_
 The pose ID of the reference (currently not changing) More...
 
okvis::PointMap landmarksMap_
 Contains all the current landmarks (synched after optimisation). More...
 
std::mutex statesMutex_
 Regulate access of landmarksMap_. More...
 
std::vector
< okvis::ExtrinsicsEstimationParameters,
Eigen::aligned_allocator
< okvis::ExtrinsicsEstimationParameters > > 
extrinsicsEstimationParametersVec_
 Extrinsics parameters. More...
 
std::vector
< okvis::ImuParameters,
Eigen::aligned_allocator
< okvis::ImuParameters > > 
imuParametersVec_
 IMU parameters. More...
 
std::shared_ptr
< ::ceres::LossFunction > 
cauchyLossFunctionPtr_
 Cauchy loss. More...
 
std::shared_ptr
< ::ceres::LossFunction > 
huberLossFunctionPtr_
 Huber loss. More...
 
std::shared_ptr
< ceres::MarginalizationError
marginalizationErrorPtr_
 The marginalisation class. More...
 
::ceres::ResidualBlockId marginalizationResidualId_
 Remembers the marginalisation object's Id. More...
 
std::unique_ptr
< okvis::ceres::CeresIterationCallback
ceresCallback_
 Maybe there was a callback registered, store it here. More...
 

Additional Inherited Members

- Public Types inherited from okvis::VioBackendInterface
enum  InitializationStatus { InitializationStatus::NotStarted = 0, InitializationStatus::Ongoing = 1, InitializationStatus::Complete = 2 }
 Enum to define the status of initialization. More...
 

Detailed Description

The estimator class.

The estimator class. This does all the backend work. Frames: W: World B: Body C: Camera S: Sensor (IMU)

Member Typedef Documentation

Union of all sensor states.

typedef std::array<StateInfo, 6> okvis::Estimator::GlobalStatesContainer
private

Container for global states.

Container for sensor states. The dimension can vary from sensor to sensor...

Member Enumeration Documentation

CameraSensorStates The camera-internal states enumerated.

Enumerator
T_SCi 

Extrinsics as T_SC.

Intrinsics 

Intrinsics.

GpsSensorStates, currently unused.

Enumerator
DynamicPressureBias 

currently unused

GlobalStates The global states enumerated.

Enumerator
T_WS 

Pose.

MagneticZBias 

Magnetometer z-bias, currently unused.

Qff 

QFF (pressure at sea level), currently unused.

T_GW 

Alignment of global frame, currently unused.

GpsSensorStates, currently unused.

Enumerator
GpsB_t_BA 

antenna offset, currently unused

ImuSensorStates The IMU-internal states enumerated.

Warning
This is slightly inconsistent, since the velocity should be global.
Enumerator
SpeedAndBias 

Speed and biases as v in S-frame, then b_g and b_a.

MagnetometerSensorStates, currently unused.

Enumerator
MagnetometerBias 

currently unused

PositionSensorStates, currently unused.

Enumerator
T_PiW 

position sensor frame to world, currently unused

PositionSensorB_t_BA 

antenna offset, currently unused

SensorStates The sensor-internal states enumerated.

Enumerator
Camera 

Camera.

Imu 

IMU.

Position 

Position, currently unused.

Gps 

GPS, currently unused.

Magnetometer 

Magnetometer, currently unused.

StaticPressure 

Static pressure, currently unused.

DynamicPressure 

Dynamic pressure, currently unused.

GpsSensorStates, currently unused.

Enumerator
StaticPressureBias 

currently unused

Constructor & Destructor Documentation

okvis::Estimator::Estimator ( )

The default constructor.

okvis::Estimator::Estimator ( std::shared_ptr< okvis::ceres::Map mapPtr)

Constructor if a ceres map is already available.

Parameters
mapPtrShared pointer to ceres map.
okvis::Estimator::~Estimator ( )
virtual

Member Function Documentation

int okvis::Estimator::addCamera ( const okvis::ExtrinsicsEstimationParameters extrinsicsEstimationParameters)
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.

Implements okvis::VioBackendInterface.

int okvis::Estimator::addImu ( const okvis::ImuParameters imuParameters)
virtual

Add an IMU to the configuration.

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

Implements okvis::VioBackendInterface.

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

Add a landmark.

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

Implements okvis::VioBackendInterface.

template<class GEOMETRY_TYPE >
::ceres::ResidualBlockId okvis::Estimator::addObservation ( uint64_t  landmarkId,
uint64_t  poseId,
size_t  camIdx,
size_t  keypointIdx 
)
template<class GEOMETRY_TYPE >
::ceres::ResidualBlockId okvis::Estimator::addObservation ( uint64_t  landmarkId,
uint64_t  poseId,
size_t  camIdx,
size_t  keypointIdx 
)

Add an observation to a landmark.

Template Parameters
GEOMETRY_TYPEThe camera geometry type for this observation.
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
Residual block ID for that observation.
bool okvis::Estimator::addStates ( okvis::MultiFramePtr  multiFrame,
const okvis::ImuMeasurementDeque imuMeasurements,
bool  asKeyframe 
)
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.

Implements okvis::VioBackendInterface.

bool okvis::Estimator::applyMarginalizationStrategy ( size_t  numKeyframes,
size_t  numImuFrames,
okvis::MapPointVector removedLandmarks 
)

Applies the dropping/marginalization strategy according to the RSS'13/IJRR'14 paper. The new number of frames in the window will be numKeyframes+numImuFrames.

Parameters
numKeyframesNumber of keyframes.
numImuFramesNumber of frames in IMU window.
removedLandmarksGet the landmarks that were removed by this operation.
Returns
True if successful.
void okvis::Estimator::clearCameras ( )
virtual

Remove all cameras from the configuration.

Implements okvis::VioBackendInterface.

void okvis::Estimator::clearImus ( )
virtual

Remove all IMUs from the configuration.

Implements okvis::VioBackendInterface.

uint64_t okvis::Estimator::currentFrameId ( ) const
virtual

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

Returns
The ID of the current frame.

Implements okvis::VioBackendInterface.

uint64_t okvis::Estimator::currentKeyframeId ( ) const

Get the ID of the current keyframe.

Returns
The ID of the current keyframe.
uint64_t okvis::Estimator::frameIdByAge ( size_t  age) const

Get the ID of an older frame.

Parameters
[in]ageage of desired frame. 0 would be the newest frame added to the state.
Returns
ID of the desired frame or 0 if parameter age was out of range.
bool okvis::Estimator::get_T_WS ( uint64_t  poseId,
okvis::kinematics::Transformation T_WS 
) const
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.

Implements okvis::VioBackendInterface.

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

Get camera states for a given pose ID.

Warning
This accesses the optimization graph, so not very fast.
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.

Implements okvis::VioBackendInterface.

template<class PARAMETER_BLOCK_T >
bool okvis::Estimator::getGlobalStateEstimateAs ( uint64_t  poseId,
int  stateType,
typename PARAMETER_BLOCK_T::estimate_t &  state 
) const
private
template<class PARAMETER_BLOCK_T >
bool okvis::Estimator::getGlobalStateParameterBlockAs ( uint64_t  poseId,
int  stateType,
PARAMETER_BLOCK_T &  stateParameterBlock 
) const
private
bool okvis::Estimator::getGlobalStateParameterBlockPtr ( uint64_t  poseId,
int  stateType,
std::shared_ptr< ceres::ParameterBlock > &  stateParameterBlockPtr 
) const
private
bool okvis::Estimator::getLandmark ( uint64_t  landmarkId,
okvis::MapPoint mapPoint 
) const
virtual

Get a specific landmark.

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

Implements okvis::VioBackendInterface.

size_t okvis::Estimator::getLandmarks ( okvis::PointMap landmarks) const
virtual

Get a copy of all the landmarks as a PointMap.

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

Implements okvis::VioBackendInterface.

size_t okvis::Estimator::getLandmarks ( okvis::MapPointVector landmarks) const

Get a copy of all the landmark in a MapPointVector. This is for legacy support. Use getLandmarks(okvis::PointMap&) if possible.

Parameters
[out]landmarksA vector of all landmarks.
See Also
getLandmarks().
Returns
number of landmarks.
template<class PARAMETER_BLOCK_T >
bool okvis::Estimator::getSensorStateEstimateAs ( uint64_t  poseId,
int  sensorIdx,
int  sensorType,
int  stateType,
typename PARAMETER_BLOCK_T::estimate_t &  state 
) const
private
template<class PARAMETER_BLOCK_T >
bool okvis::Estimator::getSensorStateParameterBlockAs ( uint64_t  poseId,
int  sensorIdx,
int  sensorType,
int  stateType,
PARAMETER_BLOCK_T &  stateParameterBlock 
) const
private
bool okvis::Estimator::getSensorStateParameterBlockPtr ( uint64_t  poseId,
int  sensorIdx,
int  sensorType,
int  stateType,
std::shared_ptr< ceres::ParameterBlock > &  stateParameterBlockPtr 
) const
private
bool okvis::Estimator::getSpeedAndBias ( uint64_t  poseId,
uint64_t  imuIdx,
okvis::SpeedAndBias speedAndBias 
) const
virtual

Get speeds and IMU biases for a given pose ID.

Warning
This accesses the optimization graph, so not very fast.
Parameters
[in]poseIdID of pose to get speeds and biases for.
[in]imuIdxindex of IMU to get biases for. As only one IMU is supported this is always 0.
[out]speedAndBiasSpeed And bias requested.
Returns
True if successful.

Implements okvis::VioBackendInterface.

bool okvis::Estimator::initPoseFromImu ( const okvis::ImuMeasurementDeque imuMeasurements,
okvis::kinematics::Transformation T_WS 
)
static

Initialise pose from IMU measurements. For convenience as static.

Parameters
[in]imuMeasurementsThe IMU measurements to be used for this.
[out]T_WSinitialised pose.
Returns
True if successful.
bool okvis::Estimator::isInImuWindow ( uint64_t  frameId) const

Checks if a particular frame is still in the IMU window.

Parameters
[in]frameIdID of frame to check.
Returns
True if the frame is in IMU window.
bool okvis::Estimator::isKeyframe ( uint64_t  frameId) const
inlinevirtual

Checks if a particular frame is a keyframe.

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

Implements okvis::VioBackendInterface.

bool okvis::Estimator::isLandmarkAdded ( uint64_t  landmarkId) const
inlinevirtual

Checks whether the landmark is added to the estimator.

Parameters
landmarkIdThe ID.
Returns
True if added.

Implements okvis::VioBackendInterface.

bool okvis::Estimator::isLandmarkInitialized ( uint64_t  landmarkId) const
virtual

Checks whether the landmark is initialized.

Parameters
landmarkIdThe ID.
Returns
True if initialised.

Implements okvis::VioBackendInterface.

okvis::MultiFramePtr okvis::Estimator::multiFrame ( uint64_t  frameId) const
inlinevirtual

Get a multiframe.

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

Implements okvis::VioBackendInterface.

size_t okvis::Estimator::numFrames ( ) const
inlinevirtual

Get the number of states/frames in the estimator.

Returns
The number of frames.

Implements okvis::VioBackendInterface.

size_t okvis::Estimator::numLandmarks ( ) const
inlinevirtual

Get the number of landmarks in the estimator.

Returns
The number of landmarks.

Implements okvis::VioBackendInterface.

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

Start ceres optimization.

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

Implements okvis::VioBackendInterface.

void okvis::Estimator::printStates ( uint64_t  poseId,
std::ostream &  buffer 
) const

Prints state information to buffer.

Parameters
poseIdThe pose Id for which to print.
bufferThe puffer to print into.
bool okvis::Estimator::removeObservation ( uint64_t  landmarkId,
uint64_t  poseId,
size_t  camIdx,
size_t  keypointIdx 
)
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.

Implements okvis::VioBackendInterface.

bool okvis::Estimator::removeObservation ( ::ceres::ResidualBlockId  residualBlockId)
private

Remove an observation from a landmark.

Parameters
residualBlockIdResidual ID for this landmark.
Returns
True if successful.
bool okvis::Estimator::set_T_WS ( uint64_t  poseId,
const okvis::kinematics::Transformation T_WS 
)
virtual

Set pose for a given pose ID.

Warning
This accesses the optimization graph, so not very fast.
Parameters
[in]poseIdID of the pose that should be changed.
[in]T_WSnew homogeneous transformation.
Returns
True if successful.

Implements okvis::VioBackendInterface.

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

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

Warning
This accesses the optimization graph, so not very fast.
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.

Implements okvis::VioBackendInterface.

template<class PARAMETER_BLOCK_T >
bool okvis::Estimator::setGlobalStateEstimateAs ( uint64_t  poseId,
int  stateType,
const typename PARAMETER_BLOCK_T::estimate_t &  state 
)
private
void okvis::Estimator::setKeyframe ( uint64_t  frameId,
bool  isKeyframe 
)
inlinevirtual

Set whether a frame is a keyframe or not.

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

Implements okvis::VioBackendInterface.

bool okvis::Estimator::setLandmark ( uint64_t  landmarkId,
const Eigen::Vector4d &  landmark 
)
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.

Implements okvis::VioBackendInterface.

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

Set the landmark initialization state.

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

Implements okvis::VioBackendInterface.

void okvis::Estimator::setMap ( std::shared_ptr< okvis::ceres::Map mapPtr)
inlinevirtual

set ceres map

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

Implements okvis::VioBackendInterface.

bool okvis::Estimator::setOptimizationTimeLimit ( double  timeLimit,
int  minIterations 
)
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.

Implements okvis::VioBackendInterface.

template<class PARAMETER_BLOCK_T >
bool okvis::Estimator::setSensorStateEstimateAs ( uint64_t  poseId,
int  sensorIdx,
int  sensorType,
int  stateType,
const typename PARAMETER_BLOCK_T::estimate_t &  state 
)
private
bool okvis::Estimator::setSpeedAndBias ( uint64_t  poseId,
size_t  imuIdx,
const okvis::SpeedAndBias speedAndBias 
)
virtual

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

Warning
This accesses the optimization graph, so not very fast.
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.

Implements okvis::VioBackendInterface.

okvis::Time okvis::Estimator::timestamp ( uint64_t  frameId) const
inlinevirtual

Get the timestamp for a particular frame.

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

Implements okvis::VioBackendInterface.

Member Data Documentation

std::shared_ptr< ::ceres::LossFunction> okvis::Estimator::cauchyLossFunctionPtr_
private

Cauchy loss.

std::unique_ptr<okvis::ceres::CeresIterationCallback> okvis::Estimator::ceresCallback_
private

Maybe there was a callback registered, store it here.

std::vector<okvis::ExtrinsicsEstimationParameters, Eigen::aligned_allocator<okvis::ExtrinsicsEstimationParameters> > okvis::Estimator::extrinsicsEstimationParametersVec_
private

Extrinsics parameters.

std::shared_ptr< ::ceres::LossFunction> okvis::Estimator::huberLossFunctionPtr_
private

Huber loss.

std::vector<okvis::ImuParameters, Eigen::aligned_allocator<okvis::ImuParameters> > okvis::Estimator::imuParametersVec_
private

IMU parameters.

okvis::PointMap okvis::Estimator::landmarksMap_
private

Contains all the current landmarks (synched after optimisation).

std::shared_ptr<okvis::ceres::Map> okvis::Estimator::mapPtr_
private

The underlying okvis::Map.

std::shared_ptr<ceres::MarginalizationError> okvis::Estimator::marginalizationErrorPtr_
private

The marginalisation class.

::ceres::ResidualBlockId okvis::Estimator::marginalizationResidualId_
private

Remembers the marginalisation object's Id.

std::map<uint64_t, okvis::MultiFramePtr> okvis::Estimator::multiFramePtrMap_
private

remember all needed okvis::MultiFrame.

uint64_t okvis::Estimator::referencePoseId_
private

The pose ID of the reference (currently not changing)

std::map<uint64_t, States> okvis::Estimator::statesMap_
private

Buffer for currently considered states.

std::mutex okvis::Estimator::statesMutex_
mutableprivate

Regulate access of landmarksMap_.


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