#include <RevoluteJointFilter.h>
Public Member Functions | |
RevoluteJointFilterPtr | clone () const |
virtual void | correctState () |
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 () |
We use the kinematic structure to predict the next pose-twist of the second rigid body of the joint (with covariance based on the model uncertainty) in sensor frame (the frame where we track the RBs). This values will be passed to the MultiRBTracker. | |
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 () |
virtual void | predictState (double time_interval_ns) |
First step when updating the filter. The next state is predicted from current state and system model. | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | RevoluteJointFilter () |
RevoluteJointFilter (const RevoluteJointFilter &rev_joint) | |
virtual void | setCovarianceDeltaMeasurementAngular (double sigma_delta_meas_uncertainty_angular) |
virtual void | setMaxRadiusDistanceRevolute (const double &value) |
virtual void | setMinRotationRevolute (const double &value) |
virtual | ~RevoluteJointFilter () |
Protected Member Functions | |
void | _initializeEKF () |
void | _initializeMeasurementModel () |
void | _initializeSystemModel () |
virtual RevoluteJointFilter * | doClone () const |
Protected Attributes | |
double | _accumulated_rotation |
BFL::ExtendedKalmanFilter * | _ekf |
BFL::AnalyticMeasurementModelGaussianUncertainty * | _meas_MODEL |
BFL::NonLinearRevoluteMeasurementPdf * | _meas_PDF |
double | _rev_max_joint_distance_for_ee |
double | _rev_min_rot_for_ee |
double | _sigma_delta_meas_uncertainty_angular |
BFL::LinearAnalyticSystemModelGaussianUncertainty * | _sys_MODEL |
BFL::LinearAnalyticConditionalGaussian * | _sys_PDF |
Definition at line 54 of file RevoluteJointFilter.h.
Constructor
Definition at line 45 of file RevoluteJointFilter.cpp.
RevoluteJointFilter::~RevoluteJointFilter | ( | ) | [virtual] |
Destructor
Definition at line 185 of file RevoluteJointFilter.cpp.
RevoluteJointFilter::RevoluteJointFilter | ( | const RevoluteJointFilter & | rev_joint | ) |
Copy constructor
Definition at line 214 of file RevoluteJointFilter.cpp.
void RevoluteJointFilter::_initializeEKF | ( | ) | [protected] |
Definition at line 151 of file RevoluteJointFilter.cpp.
void RevoluteJointFilter::_initializeMeasurementModel | ( | ) | [protected] |
Definition at line 135 of file RevoluteJointFilter.cpp.
void RevoluteJointFilter::_initializeSystemModel | ( | ) | [protected] |
Definition at line 105 of file RevoluteJointFilter.cpp.
RevoluteJointFilterPtr omip::RevoluteJointFilter::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 76 of file RevoluteJointFilter.h.
void RevoluteJointFilter::correctState | ( | ) | [virtual] |
Use the observed pose as measurement for the EKF -> Update joint parameters
Reimplemented from omip::JointFilter.
Definition at line 277 of file RevoluteJointFilter.cpp.
virtual RevoluteJointFilter* omip::RevoluteJointFilter::doClone | ( | ) | const [inline, protected, virtual] |
Implements omip::JointFilter.
Definition at line 183 of file RevoluteJointFilter.h.
void RevoluteJointFilter::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 366 of file RevoluteJointFilter.cpp.
void RevoluteJointFilter::estimateUnnormalizedModelProbability | ( | ) | [virtual] |
Reimplemented from omip::JointFilter.
Definition at line 464 of file RevoluteJointFilter.cpp.
JointFilterType RevoluteJointFilter::getJointFilterType | ( | ) | const [virtual] |
Return the joint type as one of the possible JointFilterTypes
Implements omip::JointFilter.
Definition at line 876 of file RevoluteJointFilter.cpp.
std::string RevoluteJointFilter::getJointFilterTypeStr | ( | ) | const [virtual] |
Return the joint type as an string
Implements omip::JointFilter.
Definition at line 881 of file RevoluteJointFilter.cpp.
std::vector< visualization_msgs::Marker > RevoluteJointFilter::getJointMarkersInRRBFrame | ( | ) | const [virtual] |
Return rviz markers that show the type and parameters of the estimated joint
Implements omip::JointFilter.
Definition at line 628 of file RevoluteJointFilter.cpp.
geometry_msgs::TwistWithCovariance RevoluteJointFilter::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 496 of file RevoluteJointFilter.cpp.
geometry_msgs::TwistWithCovariance RevoluteJointFilter::getPredictedSRBPoseWithCovInSensorFrame | ( | ) | [virtual] |
We use the kinematic structure to predict the next pose-twist of the second rigid body of the joint (with covariance based on the model uncertainty) in sensor frame (the frame where we track the RBs). This values will be passed to the MultiRBTracker.
Implements omip::JointFilter.
Definition at line 556 of file RevoluteJointFilter.cpp.
geometry_msgs::TwistWithCovariance RevoluteJointFilter::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 526 of file RevoluteJointFilter.cpp.
void RevoluteJointFilter::initialize | ( | ) | [virtual] |
Reimplemented from omip::JointFilter.
Definition at line 62 of file RevoluteJointFilter.cpp.
void RevoluteJointFilter::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 254 of file RevoluteJointFilter.cpp.
void RevoluteJointFilter::predictState | ( | double | time_interval_ns | ) | [virtual] |
First step when updating the filter. The next state is predicted from current state and system model.
Reimplemented from omip::JointFilter.
Definition at line 225 of file RevoluteJointFilter.cpp.
void RevoluteJointFilter::setCovarianceDeltaMeasurementAngular | ( | double | sigma_delta_meas_uncertainty_angular | ) | [virtual] |
Definition at line 57 of file RevoluteJointFilter.cpp.
void RevoluteJointFilter::setMaxRadiusDistanceRevolute | ( | const double & | value | ) | [virtual] |
Definition at line 100 of file RevoluteJointFilter.cpp.
void RevoluteJointFilter::setMinRotationRevolute | ( | const double & | value | ) | [virtual] |
Definition at line 95 of file RevoluteJointFilter.cpp.
double omip::RevoluteJointFilter::_accumulated_rotation [protected] |
Definition at line 188 of file RevoluteJointFilter.h.
BFL::ExtendedKalmanFilter* omip::RevoluteJointFilter::_ekf [protected] |
Definition at line 177 of file RevoluteJointFilter.h.
BFL::AnalyticMeasurementModelGaussianUncertainty* omip::RevoluteJointFilter::_meas_MODEL [protected] |
Definition at line 175 of file RevoluteJointFilter.h.
Definition at line 174 of file RevoluteJointFilter.h.
double omip::RevoluteJointFilter::_rev_max_joint_distance_for_ee [protected] |
Definition at line 169 of file RevoluteJointFilter.h.
double omip::RevoluteJointFilter::_rev_min_rot_for_ee [protected] |
Definition at line 168 of file RevoluteJointFilter.h.
double omip::RevoluteJointFilter::_sigma_delta_meas_uncertainty_angular [protected] |
A Revolute Joint constrains 5 of the 6 dofs of the relative motion between RBs Revolute Joints have joint orientation and joint position as motion parameters The latent variable is the angle of rotation of the second RB wrt to the reference RB
Definition at line 166 of file RevoluteJointFilter.h.
BFL::LinearAnalyticSystemModelGaussianUncertainty* omip::RevoluteJointFilter::_sys_MODEL [protected] |
Definition at line 172 of file RevoluteJointFilter.h.
BFL::LinearAnalyticConditionalGaussian* omip::RevoluteJointFilter::_sys_PDF [protected] |
Definition at line 171 of file RevoluteJointFilter.h.