#include <vo_observation.h>
Public Member Functions | |
const Eigen::Affine3d & | activeKeyframePose () const |
retrieve the last stored active keyframe pose | |
void | setKeyframeTransform (const Eigen::Affine3d &transform) |
set world transformation of current keyframe | |
void | updateJacobian (const ImuState &state) |
implemented pure virtual function for jacobian update | |
void | updateObservation (const Eigen::Affine3d &camPose) |
Update the pseudo observation based on the current camera pose given by visual odometry. | |
VOObservation () | |
~VOObservation () | |
Private Member Functions | |
void | computeDeltaFromVO (const Eigen::Affine3d &camPose) |
compute a delta observation based on the last stored base frame and the current vo estimate | |
Private Attributes | |
Eigen::Affine3d | activeKeyframe_ |
The main purpose of this class is to handle the involved relative transformations.
Definition at line 50 of file vo_observation.h.
imu_filter::VOObservation::VOObservation | ( | ) | [inline] |
Definition at line 53 of file vo_observation.h.
imu_filter::VOObservation::~VOObservation | ( | ) | [inline] |
Definition at line 60 of file vo_observation.h.
const Eigen::Affine3d& imu_filter::VOObservation::activeKeyframePose | ( | ) | const [inline] |
retrieve the last stored active keyframe pose
Definition at line 76 of file vo_observation.h.
void imu_filter::VOObservation::computeDeltaFromVO | ( | const Eigen::Affine3d & | camPose | ) | [inline, private] |
compute a delta observation based on the last stored base frame and the current vo estimate
What we want as measurement, is the relative transformation between the last keyframe of vo and the current vo estimate, which then will get applied to the IMU filter estimate corresponding to the last keyframe. This way we generate an absolute measurement from the relative ones.
Definition at line 108 of file vo_observation.h.
void imu_filter::VOObservation::setKeyframeTransform | ( | const Eigen::Affine3d & | transform | ) | [inline] |
set world transformation of current keyframe
The all generated observation will be relative to this transformation!
transform | frame transformation |
Definition at line 71 of file vo_observation.h.
void imu_filter::VOObservation::updateJacobian | ( | const ImuState & | state | ) | [inline, virtual] |
implemented pure virtual function for jacobian update
In this case the jacobian is constant
Implements imu_filter::Observation.
Definition at line 83 of file vo_observation.h.
void imu_filter::VOObservation::updateObservation | ( | const Eigen::Affine3d & | camPose | ) | [inline] |
Update the pseudo observation based on the current camera pose given by visual odometry.
camPose | current camera pose estimate from visual odometry (or somewhere else) |
Definition at line 91 of file vo_observation.h.
Eigen::Affine3d imu_filter::VOObservation::activeKeyframe_ [private] |
Definition at line 99 of file vo_observation.h.