#include <JointCombinedFilter.h>
Public Member Functions | |
| JointCombinedFilterPtr | clone () const |
| virtual void | correctState () |
| virtual void | estimateJointFilterProbabilities () |
| virtual JointCombinedFilterId | getJointCombinedFilterId () const |
| virtual JointFilterPtr | getJointFilter (JointFilterType joint_type) const |
| Get the joint filter of the type given. | |
| virtual JointFilterPtr | getMostProbableJointFilter () |
| std::map< JointFilterType, Eigen::Twistd > | getPredictedMeasurement () |
| virtual void | initialize () |
| JointCombinedFilter () | |
| JointCombinedFilter (const JointCombinedFilter &joint_combined_filter) | |
| virtual void | normalizeJointFilterProbabilities () |
| Estimate the probability of each joint filter type. NOTE: Using the Bayes Rule, the probability of a model given the data is the likelihood of the data given the model, times the (prior) probability of the model, divided by the probability of the data: p(Model|Data) = p(Data|Model) x p(Model) / p(Data) p(Data) is just a regularization term. We can ignore it and say that p(Model|Data) is proportional to... At the end, we can renormalize with the assumption that SUM_all_models( p(Model|Data) ) = 1 p(Model) is the prior probability of a certain model. We usually assume an uniform distribution, so that p(Model) = 1/(NumDifferentModels), but we can set them differently, checking that SUM_all_models(p(Model)) = 1 Finally, the probability of a model is proportional to the likelihood of the data given that model. | |
| 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. | |
| virtual void | setCovarianceAdditiveSystemNoiseJointState (const JointFilterType &joint_type, double sys_noise_js) |
| virtual void | setCovarianceAdditiveSystemNoiseJointVelocity (const JointFilterType &joint_type, double sys_noise_jv) |
| virtual void | setCovarianceAdditiveSystemNoisePhi (const JointFilterType &joint_type, double sys_noise_phi) |
| virtual void | setCovarianceAdditiveSystemNoisePx (const JointFilterType &joint_type, double sys_noise_px) |
| virtual void | setCovarianceAdditiveSystemNoisePy (const JointFilterType &joint_type, double sys_noise_py) |
| virtual void | setCovarianceAdditiveSystemNoisePz (const JointFilterType &joint_type, double sys_noise_pz) |
| virtual void | setCovarianceAdditiveSystemNoiseTheta (const JointFilterType &joint_type, double sys_noise_theta) |
| virtual void | setCovarianceDeltaMeasurementAngular (double sigma_delta_meas_uncertainty_angular) |
| virtual void | setCovarianceDeltaMeasurementLinear (double sigma_delta_meas_uncertainty_linear) |
| virtual void | setCovarianceMeasurementNoise (const JointFilterType &joint_type, double meas_noise) |
| virtual void | setCovariancePrior (const JointFilterType &joint_type, 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) |
| void | setJointCombinedFilterId (JointCombinedFilterId new_joint_combined_filter_id) |
| virtual void | setJointLikelihoodDisconnected (double disconnected_joint_likelihood) |
| virtual void | setLoopPeriodNS (double loop_period_ns) |
| virtual void | setMaxRadiusDistanceRevolute (const double &value) |
| virtual void | setMaxRotationRigid (double rig_max_rotation) |
| virtual void | setMaxTranslationRigid (double rig_max_translation) |
| virtual void | setMeasurement (joint_measurement_t acquired_measurement, const double &measurement_timestamp_ns) |
| Set the latest acquired measurement. | |
| virtual void | setMinRotationRevolute (const double &value) |
| virtual void | setNormalizingTerm (double normalizing_term) |
| virtual void | setNumSamplesForLikelihoodEstimation (int likelihood_sample_num) |
| virtual | ~JointCombinedFilter () |
Protected Member Functions | |
| virtual JointCombinedFilter * | doClone () const |
Protected Attributes | |
| joint_filters_map | _joint_filters |
| JointCombinedFilterId | _joint_id |
| std::map< JointFilterType, double > | _joint_normalized_prediction_errors |
| double | _normalizing_term |
Static Protected Attributes | |
| static JointCombinedFilterId | _joint_id_generator = 1 |
Class JointCombinedFilter It contains one of instance of each joint filter type and updates them with each acquired measurement The measurements are RB poses: one is the reference RB and the other is the called "Second RB" Each joint filter type defines a different motion constraint Each joint filter type tracks a different set of parameters (joint parameters) Each joint filter type generates a measurement prediction (pose of one of the rigid bodies given the motion of the reference body)
Definition at line 70 of file JointCombinedFilter.h.
Default Constructor
Definition at line 16 of file JointCombinedFilter.cpp.
| JointCombinedFilter::~JointCombinedFilter | ( | ) | [virtual] |
Destructor
Definition at line 160 of file JointCombinedFilter.cpp.
| JointCombinedFilter::JointCombinedFilter | ( | const JointCombinedFilter & | joint_combined_filter | ) |
Copy constructor
Definition at line 165 of file JointCombinedFilter.cpp.
| JointCombinedFilterPtr omip::JointCombinedFilter::clone | ( | ) | const [inline] |
Creates a new JointCombinedFilter object with the same values as this one and passes its reference
Definition at line 92 of file JointCombinedFilter.h.
| void JointCombinedFilter::correctState | ( | ) | [virtual] |
Refine the parameters of the joint based on the difference between predicted and measured pose of the second rigid body
Definition at line 199 of file JointCombinedFilter.cpp.
| virtual JointCombinedFilter* omip::JointCombinedFilter::doClone | ( | ) | const [inline, protected, virtual] |
Definition at line 225 of file JointCombinedFilter.h.
| void JointCombinedFilter::estimateJointFilterProbabilities | ( | ) | [virtual] |
Definition at line 246 of file JointCombinedFilter.cpp.
| JointCombinedFilterId JointCombinedFilter::getJointCombinedFilterId | ( | ) | const [virtual] |
Return the joint id
Definition at line 277 of file JointCombinedFilter.cpp.
| JointFilterPtr JointCombinedFilter::getJointFilter | ( | JointFilterType | joint_type | ) | const [virtual] |
Get the joint filter of the type given.
| joint_type | Type of the joint filter to get |
Definition at line 272 of file JointCombinedFilter.cpp.
| JointFilterPtr JointCombinedFilter::getMostProbableJointFilter | ( | ) | [virtual] |
Obtain a pointer to the most probable joint filter, given the likelihood of the last measurements for each joint filter and the prior probability of each filter type
Definition at line 256 of file JointCombinedFilter.cpp.
| std::map< JointFilterType, Eigen::Twistd > JointCombinedFilter::getPredictedMeasurement | ( | ) |
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)
Definition at line 181 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::initialize | ( | ) | [virtual] |
Definition at line 75 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::normalizeJointFilterProbabilities | ( | ) | [virtual] |
Estimate the probability of each joint filter type. NOTE: Using the Bayes Rule, the probability of a model given the data is the likelihood of the data given the model, times the (prior) probability of the model, divided by the probability of the data: p(Model|Data) = p(Data|Model) x p(Model) / p(Data) p(Data) is just a regularization term. We can ignore it and say that p(Model|Data) is proportional to... At the end, we can renormalize with the assumption that SUM_all_models( p(Model|Data) ) = 1 p(Model) is the prior probability of a certain model. We usually assume an uniform distribution, so that p(Model) = 1/(NumDifferentModels), but we can set them differently, checking that SUM_all_models(p(Model)) = 1 Finally, the probability of a model is proportional to the likelihood of the data given that model.
Definition at line 215 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::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)
Definition at line 173 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::predictState | ( | double | time_interval_ns | ) | [virtual] |
First step when updating the filter. The next state is predicted from current state and system model.
Definition at line 207 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setCovarianceAdditiveSystemNoiseJointState | ( | const JointFilterType & | joint_type, |
| double | sys_noise_js | ||
| ) | [virtual] |
Definition at line 133 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setCovarianceAdditiveSystemNoiseJointVelocity | ( | const JointFilterType & | joint_type, |
| double | sys_noise_jv | ||
| ) | [virtual] |
Definition at line 138 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setCovarianceAdditiveSystemNoisePhi | ( | const JointFilterType & | joint_type, |
| double | sys_noise_phi | ||
| ) | [virtual] |
Definition at line 108 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setCovarianceAdditiveSystemNoisePx | ( | const JointFilterType & | joint_type, |
| double | sys_noise_px | ||
| ) | [virtual] |
Definition at line 118 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setCovarianceAdditiveSystemNoisePy | ( | const JointFilterType & | joint_type, |
| double | sys_noise_py | ||
| ) | [virtual] |
Definition at line 123 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setCovarianceAdditiveSystemNoisePz | ( | const JointFilterType & | joint_type, |
| double | sys_noise_pz | ||
| ) | [virtual] |
Definition at line 128 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setCovarianceAdditiveSystemNoiseTheta | ( | const JointFilterType & | joint_type, |
| double | sys_noise_theta | ||
| ) | [virtual] |
Definition at line 113 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setCovarianceDeltaMeasurementAngular | ( | double | sigma_delta_meas_uncertainty_angular | ) | [virtual] |
Definition at line 70 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setCovarianceDeltaMeasurementLinear | ( | double | sigma_delta_meas_uncertainty_linear | ) | [virtual] |
Definition at line 65 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setCovarianceMeasurementNoise | ( | const JointFilterType & | joint_type, |
| double | meas_noise | ||
| ) | [virtual] |
Definition at line 143 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setCovariancePrior | ( | const JointFilterType & | joint_type, |
| double | prior_cov_vel | ||
| ) | [virtual] |
Definition at line 103 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::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] |
Definition at line 148 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setJointCombinedFilterId | ( | JointCombinedFilterId | new_joint_combined_filter_id | ) |
Definition at line 282 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setJointLikelihoodDisconnected | ( | double | disconnected_joint_likelihood | ) | [virtual] |
Definition at line 36 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setLoopPeriodNS | ( | double | loop_period_ns | ) | [virtual] |
Definition at line 41 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setMaxRadiusDistanceRevolute | ( | const double & | value | ) | [virtual] |
Definition at line 98 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setMaxRotationRigid | ( | double | rig_max_rotation | ) | [virtual] |
Definition at line 88 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setMaxTranslationRigid | ( | double | rig_max_translation | ) | [virtual] |
Definition at line 83 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setMeasurement | ( | joint_measurement_t | acquired_measurement, |
| const double & | measurement_timestamp_ns | ||
| ) | [virtual] |
Set the latest acquired measurement.
| acquired_measurement | Latest acquired measurement |
Definition at line 191 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setMinRotationRevolute | ( | const double & | value | ) | [virtual] |
Definition at line 93 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setNormalizingTerm | ( | double | normalizing_term | ) | [virtual] |
Definition at line 57 of file JointCombinedFilter.cpp.
| void JointCombinedFilter::setNumSamplesForLikelihoodEstimation | ( | int | likelihood_sample_num | ) | [virtual] |
Definition at line 49 of file JointCombinedFilter.cpp.
joint_filters_map omip::JointCombinedFilter::_joint_filters [protected] |
Definition at line 220 of file JointCombinedFilter.h.
JointCombinedFilterId omip::JointCombinedFilter::_joint_id [protected] |
Definition at line 215 of file JointCombinedFilter.h.
JointCombinedFilterId JointCombinedFilter::_joint_id_generator = 1 [static, protected] |
Definition at line 213 of file JointCombinedFilter.h.
std::map<JointFilterType, double> omip::JointCombinedFilter::_joint_normalized_prediction_errors [protected] |
Definition at line 223 of file JointCombinedFilter.h.
double omip::JointCombinedFilter::_normalizing_term [protected] |
Definition at line 217 of file JointCombinedFilter.h.