#include <RigidJointFilter.h>
Public Member Functions | |
RigidJointFilterPtr | clone () const |
virtual void | estimateMeasurementHistoryLikelihood () |
virtual void | estimateUnnormalizedModelProbability () |
virtual JointFilterType | getJointFilterType () const |
virtual std::string | getJointFilterTypeStr () const |
virtual std::vector < visualization_msgs::Marker > | getJointMarkersInRRBFrame () const |
virtual geometry_msgs::TwistWithCovariance | getPredictedSRBDeltaPoseWithCovInSensorFrame () |
Generate a prediction about the change in pose of the second rigid body (SRB) and its covariance using the frame of sensor (SF) as observation and ref frame based on the predicted measurement and the predicted next pose of the reference rigid body (RRB) in the sensor frame (SF) | |
virtual geometry_msgs::TwistWithCovariance | getPredictedSRBPoseWithCovInSensorFrame () |
virtual geometry_msgs::TwistWithCovariance | getPredictedSRBVelocityWithCovInSensorFrame () |
Generate a prediction about the velocity of the second rigid body (SRB) and its covariance using the frame of sensor (SF) as observation and ref frame based on the predicted measurement and the predicted next pose of the reference rigid body (RRB) in the sensor frame (SF) | |
virtual void | initialize () |
virtual void | predictMeasurement () |
RigidJointFilter () | |
RigidJointFilter (const RigidJointFilter &rigid_joint) | |
virtual void | setMaxRotationRigid (double max_rot) |
virtual void | setMaxTranslationRigid (double max_trans) |
virtual | ~RigidJointFilter () |
Protected Member Functions | |
virtual RigidJointFilter * | doClone () const |
Protected Attributes | |
double | _motion_memory_prior |
double | _rig_max_rotation |
double | _rig_max_translation |
Definition at line 43 of file RigidJointFilter.h.
Constructor
Definition at line 5 of file RigidJointFilter.cpp.
RigidJointFilter::~RigidJointFilter | ( | ) | [virtual] |
Destructor
Definition at line 13 of file RigidJointFilter.cpp.
RigidJointFilter::RigidJointFilter | ( | const RigidJointFilter & | rigid_joint | ) |
Copy constructor
Definition at line 18 of file RigidJointFilter.cpp.
RigidJointFilterPtr omip::RigidJointFilter::clone | ( | ) | const [inline] |
Creates a new Joint object with the same values as this one and passes its reference
Reimplemented from omip::JointFilter.
Definition at line 63 of file RigidJointFilter.h.
virtual RigidJointFilter* omip::RigidJointFilter::doClone | ( | ) | const [inline, protected, virtual] |
A Rigid Joint constrains the 6 dofs of the relative motion between RBs Rigid Joints have no motion parameters Rigid Joints have no latent variables
Implements omip::JointFilter.
Definition at line 139 of file RigidJointFilter.h.
void RigidJointFilter::estimateMeasurementHistoryLikelihood | ( | ) | [virtual] |
Measure the error of the last joint parameters by testing them against the full observed trajectory of the second RB relative to the reference RB
Reimplemented from omip::JointFilter.
Definition at line 51 of file RigidJointFilter.cpp.
void RigidJointFilter::estimateUnnormalizedModelProbability | ( | ) | [virtual] |
Reimplemented from omip::JointFilter.
Definition at line 127 of file RigidJointFilter.cpp.
JointFilterType RigidJointFilter::getJointFilterType | ( | ) | const [virtual] |
Return the joint type as one of the possible JointFilterTypes
Implements omip::JointFilter.
Definition at line 315 of file RigidJointFilter.cpp.
std::string RigidJointFilter::getJointFilterTypeStr | ( | ) | const [virtual] |
Return the joint type as an string
Implements omip::JointFilter.
Definition at line 320 of file RigidJointFilter.cpp.
std::vector< visualization_msgs::Marker > RigidJointFilter::getJointMarkersInRRBFrame | ( | ) | const [virtual] |
Return rviz markers that show the type and parameters of the estimated joint
Implements omip::JointFilter.
Definition at line 239 of file RigidJointFilter.cpp.
geometry_msgs::TwistWithCovariance RigidJointFilter::getPredictedSRBDeltaPoseWithCovInSensorFrame | ( | ) | [virtual] |
Generate a prediction about the change in pose of the second rigid body (SRB) and its covariance using the frame of sensor (SF) as observation and ref frame based on the predicted measurement and the predicted next pose of the reference rigid body (RRB) in the sensor frame (SF)
Implements omip::JointFilter.
Definition at line 132 of file RigidJointFilter.cpp.
geometry_msgs::TwistWithCovariance RigidJointFilter::getPredictedSRBPoseWithCovInSensorFrame | ( | ) | [virtual] |
Generate a hypothesis about the pose of the second rigid body based on the pose of the reference rigid body and the internal state (joint parameters and latent variable)
Implements omip::JointFilter.
Definition at line 184 of file RigidJointFilter.cpp.
geometry_msgs::TwistWithCovariance RigidJointFilter::getPredictedSRBVelocityWithCovInSensorFrame | ( | ) | [virtual] |
Generate a prediction about the velocity of the second rigid body (SRB) and its covariance using the frame of sensor (SF) as observation and ref frame based on the predicted measurement and the predicted next pose of the reference rigid body (RRB) in the sensor frame (SF)
Implements omip::JointFilter.
Definition at line 158 of file RigidJointFilter.cpp.
void RigidJointFilter::initialize | ( | ) | [virtual] |
Reimplemented from omip::JointFilter.
Definition at line 23 of file RigidJointFilter.cpp.
void RigidJointFilter::predictMeasurement | ( | ) | [virtual] |
Generate a hypothesis about the pose of the second rigid body based on the pose of the reference rigid body and the internal state (joint parameters and latent variable)
Reimplemented from omip::JointFilter.
Definition at line 40 of file RigidJointFilter.cpp.
void RigidJointFilter::setMaxRotationRigid | ( | double | max_rot | ) | [virtual] |
Definition at line 35 of file RigidJointFilter.cpp.
void RigidJointFilter::setMaxTranslationRigid | ( | double | max_trans | ) | [virtual] |
Definition at line 30 of file RigidJointFilter.cpp.
double omip::RigidJointFilter::_motion_memory_prior [protected] |
Definition at line 147 of file RigidJointFilter.h.
double omip::RigidJointFilter::_rig_max_rotation [protected] |
Definition at line 145 of file RigidJointFilter.h.
double omip::RigidJointFilter::_rig_max_translation [protected] |
Definition at line 144 of file RigidJointFilter.h.