#include <DisconnectedJointFilter.h>
Public Member Functions | |
| DisconnectedJointFilterPtr | clone () const |
| DisconnectedJointFilter () | |
| DisconnectedJointFilter (const DisconnectedJointFilter &rigid_joint) | |
| 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) More... | |
| virtual geometry_msgs::TwistWithCovariance | getPredictedSRBPoseWithCovInSensorFrame () |
| Generate a prediction about the pose-twist 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) More... | |
| 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) More... | |
| virtual double | getProbabilityOfJointFilter () const |
| Return the normalized joint filter probability of the Disconnected Joint model, which is a constant value we set at the beginning and use as quality threshold for the other models. More... | |
| virtual void | initialize () |
| virtual | ~DisconnectedJointFilter () |
 Public Member Functions inherited from omip::JointFilter | |
| JointFilterPtr | clone () const |
| Creates a new Joint object with the same values as this one and passes its reference. More... | |
| virtual void | correctState () |
| Correct the state of the filter (the joint parameters) based on the difference between the predicted and the last acquired measurement It is only implemented for revolute and prismatic because rigid and disconnected have no parameters to correct. More... | |
| virtual void | estimateMeasurementHistoryLikelihood () |
| Estimate the error of the last joint parameters by testing them against the full observed trajectory of the second RB relative to the reference RB -> Obtain the likelihood of the measurements given the model and the model parameters Implemented for prismatic, revolute and rigid joints, but not for disconnected (there is no way to estimate its likelihood, we use a value that is a threshold) More... | |
| virtual void | estimateUnnormalizedModelProbability () |
| virtual double | getCovarianceOrientationPhiPhiInRRBFrame () const |
| virtual double | getCovarianceOrientationPhiThetaInRRBFrame () const |
| virtual double | getCovarianceOrientationThetaThetaInRRBFrame () const |
| virtual JointCombinedFilterId | getJointId () const |
| Get the unique Id that identifies this joint. More... | |
| virtual Eigen::Vector3d | getJointOrientationRPYInRRBFrame () const |
| virtual Eigen::Vector3d | getJointOrientationUnitaryVector () const |
| virtual Eigen::Vector3d | getJointPositionInRRBFrame () const |
| virtual double | getJointState () const |
| Return the joint state. Useful for plotting and testing purposes (monitoring action) More... | |
| virtual double | getJointVelocity () const |
| Return the joint velocity. Useful for plotting and testing purposes (monitoring action) More... | |
| virtual double | getLikelihoodOfLastMeasurements () const |
| Return the estimated error of the last joint parameters by testing them against the full observed trajectory. More... | |
| virtual double | getLoopPeriodNS () const |
| virtual double | getModelPriorProbability () const |
| Get the prior probability of this type of model/joint. We could use this in the feature if the robot is in environments where is more probable to encounter joints of certain type. More... | |
| virtual double | getNormalizingTerm () |
| virtual int | getNumSamplesForLikelihoodEstimation () const |
| virtual double | getOrientationPhiInRRBFrame () const |
| virtual double | getOrientationThetaInRRBFrame () const |
| virtual Eigen::Twistd | getPredictedMeasurement () const |
| Get the predicted pose-twist of the second rigid body (SRB) using the frame of the reference rigid body (RRBF) as observation and ref frame. More... | |
| virtual double | getUnnormalizedProbabilityOfJointFilter () const |
| Return the unnormalized joint filter probability as p(Data|Model) x p(Model) More... | |
| JointFilter () | |
| Constructor. More... | |
| JointFilter (const JointFilter &joint) | |
| Copy constructor. More... | |
| virtual void | predictMeasurement () |
| Generate a prediction about the pose-twist of the second rigid body (SRB) using the frame of the reference rigid body (RRBF) as observation and ref frame based on the belief state (joint parameters and latent variable) and the initial pose of the SRB in the RRBF Implemented for prismatic, revolute and rigid joints, but not for disconnected (there is no way to predict the next measurement because there is no constraint in the motion if the joint is disconnected) More... | |
| virtual void | predictState (double time_interval_ns) |
| Predict the next state. More... | |
| virtual void | setCovarianceAdditiveSystemNoiseJointState (double sigma_sys_noise_pv) |
| virtual void | setCovarianceAdditiveSystemNoiseJointVelocity (double sigma_sys_noise_pvd) |
| virtual void | setCovarianceAdditiveSystemNoisePhi (double sigma_sys_noise_phi) |
| virtual void | setCovarianceAdditiveSystemNoisePx (double sigma_sys_noise_px) |
| virtual void | setCovarianceAdditiveSystemNoisePy (double sigma_sys_noise_py) |
| virtual void | setCovarianceAdditiveSystemNoisePz (double sigma_sys_noise_pz) |
| virtual void | setCovarianceAdditiveSystemNoiseTheta (double sigma_sys_noise_theta) |
| virtual void | setCovarianceMeasurementNoise (double sigma_meas_noise) |
| virtual void | setCovariancePrior (double prior_cov_vel) |
| virtual void | setInitialMeasurement (const joint_measurement_t &initial_measurement, const Eigen::Twistd &rrb_pose_at_srb_birth_in_sf, const Eigen::Twistd &srb_pose_at_srb_birth_in_sf) |
| virtual void | setJointId (JointCombinedFilterId joint_id) |
| virtual void | setLikelihoodOfLastMeasurements (double likelihood) |
| Set the estimated error of the last joint parameters (only useful for the disconnected joint because we do not estimate the likelihood internally) More... | |
| virtual void | setLoopPeriodNS (double loop_period_ns) |
| virtual void | setMeasurement (joint_measurement_t acquired_measurement, const double &measurement_timestamp_ns) |
| Set the latest acquired measurement. More... | |
| virtual void | setModelPriorProbability (double model_prior_probability) |
| Set the prior probability of this type of model/joint. We could use this in the feature if the robot is in environments where is more probable to encounter joints of certain type. More... | |
| virtual void | setNormalizingTerm (double normalizing_term) |
| Set the normalizing term from the combined filter as the sum of all unnormalized probabilities. More... | |
| virtual void | setNumSamplesForLikelihoodEstimation (int likelihood_sample_num) |
| virtual | ~JointFilter () |
| Destructor. More... | |
Protected Member Functions | |
| virtual DisconnectedJointFilter * | doClone () const |
| Protected Member Functions inherited from omip::JointFilter | |
| void | _initVariables (JointCombinedFilterId joint_id) |
| Initialize all variables of the filter. More... | |
Detailed Description
Definition at line 43 of file DisconnectedJointFilter.h.
Constructor & Destructor Documentation
| DisconnectedJointFilter::DisconnectedJointFilter | ( | ) |
Constructor
Definition at line 7 of file DisconnectedJointFilter.cpp.
|
virtual |
Destructor
Definition at line 13 of file DisconnectedJointFilter.cpp.
| DisconnectedJointFilter::DisconnectedJointFilter | ( | const DisconnectedJointFilter & | rigid_joint | ) |
Copy constructor
Definition at line 18 of file DisconnectedJointFilter.cpp.
Member Function Documentation
|
inline |
Creates a new Joint object with the same values as this one and passes its reference
- Returns
- - A reference to the clone of this Feature object
Definition at line 63 of file DisconnectedJointFilter.h.
|
inlineprotectedvirtual |
A Disconnected Joint does not constrain any of the 6 dofs of the relative motion between RBs Disconnected Joints have no motion parameters Disconnected Joints have no latent variables
Implements omip::JointFilter.
Definition at line 133 of file DisconnectedJointFilter.h.
|
virtual |
Return the joint type as one of the possible JointFilterTypes
Implements omip::JointFilter.
Definition at line 203 of file DisconnectedJointFilter.cpp.
|
virtual |
Return the joint type as an string
Implements omip::JointFilter.
Definition at line 208 of file DisconnectedJointFilter.cpp.
|
virtual |
Return rviz markers that show the type and parameters of the estimated joint
Implements omip::JointFilter.
Definition at line 167 of file DisconnectedJointFilter.cpp.
|
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)
- Returns
- geometry_msgs::TwistWithCovariance Change in pose of the second rigid body in form of a twist with covariance based on the model uncertainty
Implements omip::JointFilter.
Definition at line 33 of file DisconnectedJointFilter.cpp.
|
virtual |
Generate a prediction about the pose-twist 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 103 of file DisconnectedJointFilter.cpp.
|
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)
- Returns
- geometry_msgs::TwistWithCovariance Velocity of the second rigid body in form of a twist with covariance based on the model uncertainty
Implements omip::JointFilter.
Definition at line 68 of file DisconnectedJointFilter.cpp.
|
virtual |
Return the normalized joint filter probability of the Disconnected Joint model, which is a constant value we set at the beginning and use as quality threshold for the other models.
- Returns
- double Normalized probability of this joint filter
Reimplemented from omip::JointFilter.
Definition at line 28 of file DisconnectedJointFilter.cpp.
|
virtual |
Reimplemented from omip::JointFilter.
Definition at line 23 of file DisconnectedJointFilter.cpp.
The documentation for this class was generated from the following files: