base
|
#include <PoseWithCovariance.hpp>
Public Member Functions | |
PoseWithCovariance () | |
PoseWithCovariance (const base::TransformWithCovariance &transform) | |
PoseWithCovariance (const base::samples::RigidBodyState &rbs) | |
PoseWithCovariance | operator* (const PoseWithCovariance &pose) const |
void | setTransform (const base::TransformWithCovariance &transform) |
void | setTransform (const Eigen::Affine3d &transform) |
const base::TransformWithCovariance & | getTransformWithCovariance () const |
Eigen::Affine3d | getTransform () const |
const base::TransformWithCovariance::Covariance & | getCovariance () const |
base::samples::RigidBodyState | toRigidBodyState () const |
Public Attributes | |
base::Time | time |
std::string | frame_id |
std::string | object_frame_id |
base::TransformWithCovariance | transform |
Represents a 3D pose with uncertainty of the frame 'object_frame_id' in the frame 'frame_id'. A frame ID shall be globally unique and correspond to a coordinate system. Following the Rock's conventions coordinate systems are right handed with X forward, Y left and Z up.
The pose in this structure transforms a vector
from the object frame to the reference frame:
Example of a transformation chain:
base::samples::PoseWithCovariance::PoseWithCovariance | ( | ) |
Default constructor
|
explicit |
Initializes type from a TransformWithCovariance
|
explicit |
Initializes type from a RigidBodyState
const TransformWithCovariance::Covariance & base::samples::PoseWithCovariance::getCovariance | ( | ) | const |
Returns the 6x6 covariance matrix
Eigen::Affine3d base::samples::PoseWithCovariance::getTransform | ( | ) | const |
Returns transformation as Eigen::Affine3d
const TransformWithCovariance & base::samples::PoseWithCovariance::getTransformWithCovariance | ( | ) | const |
Returns transformation as TransformWithCovariance
PoseWithCovariance base::samples::PoseWithCovariance::operator* | ( | const PoseWithCovariance & | pose | ) | const |
Performs a composition of this pose with a given pose. The result is another pose with result = this * pose. The frame ID's will be set accordingly (e.g. a_in_b = c_in_b * a_in_c). Note that this method doesn't check if the composition is frame wise valid.
void base::samples::PoseWithCovariance::setTransform | ( | const base::TransformWithCovariance & | transform | ) |
Sets the transformation as TransformWithCovariance
void base::samples::PoseWithCovariance::setTransform | ( | const Eigen::Affine3d & | transform | ) |
Sets the transformation as Eigen::Affine3d
RigidBodyState base::samples::PoseWithCovariance::toRigidBodyState | ( | ) | const |
Converts the type to a RigidBodyState. Note that the cross-covariances will be lost, since the RigidBodyState does not store them.
std::string base::samples::PoseWithCovariance::frame_id |
ID of the reference frame where this pose is expressed in
std::string base::samples::PoseWithCovariance::object_frame_id |
ID of the object frame defined by this pose
base::Time base::samples::PoseWithCovariance::time |
Reference timestamp of the pose sample
base::TransformWithCovariance base::samples::PoseWithCovariance::transform |
Pose of the object frame in the reference frame