OKVIS ROS
|
The estimator class. More...
#include <Estimator.hpp>
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< StateInfo > | SpecificSensorStatesContainer |
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, States > | statesMap_ |
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... | |
The estimator class.
The estimator class. This does all the backend work. Frames: W: World B: Body C: Camera S: Sensor (IMU)
|
private |
Union of all sensor states.
|
private |
Container for global states.
|
private |
Container for sensor states. The dimension can vary from sensor to sensor...
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
okvis::Estimator::Estimator | ( | ) |
The default constructor.
okvis::Estimator::Estimator | ( | std::shared_ptr< okvis::ceres::Map > | mapPtr | ) |
Constructor if a ceres map is already available.
mapPtr | Shared pointer to ceres map. |
|
virtual |
|
virtual |
Add a camera to the configuration. Sensors can only be added and never removed.
extrinsicsEstimationParameters | The parameters that tell how to estimate extrinsics. |
Implements okvis::VioBackendInterface.
|
virtual |
Add an IMU to the configuration.
imuParameters | The IMU parameters. |
Implements okvis::VioBackendInterface.
|
virtual |
Add a landmark.
landmarkId | ID of the new landmark. |
landmark | Homogeneous coordinates of landmark in W-frame. |
Implements okvis::VioBackendInterface.
::ceres::ResidualBlockId okvis::Estimator::addObservation | ( | uint64_t | landmarkId, |
uint64_t | poseId, | ||
size_t | camIdx, | ||
size_t | keypointIdx | ||
) |
::ceres::ResidualBlockId okvis::Estimator::addObservation | ( | uint64_t | landmarkId, |
uint64_t | poseId, | ||
size_t | camIdx, | ||
size_t | keypointIdx | ||
) |
Add an observation to a landmark.
GEOMETRY_TYPE | The camera geometry type for this observation. |
landmarkId | ID of landmark. |
poseId | ID of pose where the landmark was observed. |
camIdx | ID of camera frame where the landmark was observed. |
keypointIdx | ID of keypoint corresponding to the landmark. |
|
virtual |
Add a pose to the state.
multiFrame | Matched multiFrame. |
imuMeasurements | IMU measurements from last state to new one. |
asKeyframe | Is this new frame a keyframe? |
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.
numKeyframes | Number of keyframes. |
numImuFrames | Number of frames in IMU window. |
removedLandmarks | Get the landmarks that were removed by this operation. |
|
virtual |
Remove all cameras from the configuration.
Implements okvis::VioBackendInterface.
|
virtual |
Remove all IMUs from the configuration.
Implements okvis::VioBackendInterface.
|
virtual |
Get the ID of the newest frame added to the state.
Implements okvis::VioBackendInterface.
uint64_t okvis::Estimator::currentKeyframeId | ( | ) | const |
Get the ID of the current keyframe.
uint64_t okvis::Estimator::frameIdByAge | ( | size_t | age | ) | const |
Get the ID of an older frame.
[in] | age | age of desired frame. 0 would be the newest frame added to the state. |
|
virtual |
Get pose for a given pose ID.
[in] | poseId | ID of desired pose. |
[out] | T_WS | Homogeneous transformation of this pose. |
Implements okvis::VioBackendInterface.
|
virtual |
Get camera states for a given pose ID.
[in] | poseId | ID of pose to get camera state for. |
[in] | cameraIdx | index of camera to get state for. |
[out] | T_SCi | Homogeneous transformation from sensor (IMU) frame to camera frame. |
Implements okvis::VioBackendInterface.
|
private |
|
private |
|
private |
|
virtual |
Get a specific landmark.
[in] | landmarkId | ID of desired landmark. |
[out] | mapPoint | Landmark information, such as quality, coordinates etc. |
Implements okvis::VioBackendInterface.
|
virtual |
Get a copy of all the landmarks as a PointMap.
[out] | landmarks | The 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.
[out] | landmarks | A vector of all landmarks. |
|
private |
|
private |
|
private |
|
virtual |
Get speeds and IMU biases for a given pose ID.
[in] | poseId | ID of pose to get speeds and biases for. |
[in] | imuIdx | index of IMU to get biases for. As only one IMU is supported this is always 0. |
[out] | speedAndBias | Speed And bias requested. |
Implements okvis::VioBackendInterface.
|
static |
Initialise pose from IMU measurements. For convenience as static.
[in] | imuMeasurements | The IMU measurements to be used for this. |
[out] | T_WS | initialised pose. |
bool okvis::Estimator::isInImuWindow | ( | uint64_t | frameId | ) | const |
Checks if a particular frame is still in the IMU window.
[in] | frameId | ID of frame to check. |
|
inlinevirtual |
Checks if a particular frame is a keyframe.
[in] | frameId | ID of frame to check. |
Implements okvis::VioBackendInterface.
|
inlinevirtual |
Checks whether the landmark is added to the estimator.
landmarkId | The ID. |
Implements okvis::VioBackendInterface.
|
virtual |
Checks whether the landmark is initialized.
landmarkId | The ID. |
Implements okvis::VioBackendInterface.
|
inlinevirtual |
Get a multiframe.
frameId | ID of desired multiframe. |
Implements okvis::VioBackendInterface.
|
inlinevirtual |
Get the number of states/frames in the estimator.
Implements okvis::VioBackendInterface.
|
inlinevirtual |
Get the number of landmarks in the estimator.
Implements okvis::VioBackendInterface.
|
virtual |
Start ceres optimization.
[in] | numIter | Maximum number of iterations. |
[in] | numThreads | Number of threads. |
[in] | verbose | Print 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.
poseId | The pose Id for which to print. |
buffer | The puffer to print into. |
|
virtual |
Remove an observation from a landmark, if available.
landmarkId | ID of landmark. |
poseId | ID of pose where the landmark was observed. |
camIdx | ID of camera frame where the landmark was observed. |
keypointIdx | ID of keypoint corresponding to the landmark. |
Implements okvis::VioBackendInterface.
|
private |
Remove an observation from a landmark.
residualBlockId | Residual ID for this landmark. |
|
virtual |
Set pose for a given pose ID.
[in] | poseId | ID of the pose that should be changed. |
[in] | T_WS | new homogeneous transformation. |
Implements okvis::VioBackendInterface.
|
virtual |
Set the transformation from sensor to camera frame for a given pose ID.
[in] | poseId | ID of the pose to change corresponding camera states for. |
[in] | cameraIdx | Index of camera to set state for. |
[in] | T_SCi | new homogeneous transformation from sensor (IMU) to camera frame. |
Implements okvis::VioBackendInterface.
|
private |
|
inlinevirtual |
Set whether a frame is a keyframe or not.
[in] | frameId | The frame ID. |
[in] | isKeyframe | Whether or not keyrame. |
Implements okvis::VioBackendInterface.
|
virtual |
Set the homogeneous coordinates for a landmark.
[in] | landmarkId | The landmark ID. |
[in] | landmark | Homogeneous coordinates of landmark in W-frame. |
Implements okvis::VioBackendInterface.
|
virtual |
Set the landmark initialization state.
[in] | landmarkId | The landmark ID. |
[in] | initialized | Whether or not initialised. |
Implements okvis::VioBackendInterface.
|
inlinevirtual |
set ceres map
[in] | mapPtr | The pointer to the okvis::ceres::Map. |
Implements okvis::VioBackendInterface.
|
virtual |
Set a time limit for the optimization process.
[in] | timeLimit | Time limit in seconds. If timeLimit < 0 the time limit is removed. |
[in] | minIterations | minimum iterations the optimization process should do disregarding the time limit. |
Implements okvis::VioBackendInterface.
|
private |
|
virtual |
Set the speeds and IMU biases for a given pose ID.
[in] | poseId | ID of the pose to change corresponding speeds and biases for. |
[in] | imuIdx | index of IMU to get biases for. As only one IMU is supported this is always 0. |
[in] | speedAndBias | new speeds and biases. |
Implements okvis::VioBackendInterface.
|
inlinevirtual |
Get the timestamp for a particular frame.
[in] | frameId | ID of frame. |
Implements okvis::VioBackendInterface.
|
private |
Cauchy loss.
|
private |
Maybe there was a callback registered, store it here.
|
private |
Extrinsics parameters.
|
private |
Huber loss.
|
private |
IMU parameters.
|
private |
Contains all the current landmarks (synched after optimisation).
|
private |
The underlying okvis::Map.
|
private |
The marginalisation class.
|
private |
Remembers the marginalisation object's Id.
|
private |
remember all needed okvis::MultiFrame.
|
private |
The pose ID of the reference (currently not changing)
|
private |
Buffer for currently considered states.
|
mutableprivate |
Regulate access of landmarksMap_.