Go to the documentation of this file.00001 #include <boost/foreach.hpp>
00002
00003 #include "joint_tracker/JointCombinedFilter.h"
00004 #include "joint_tracker/JointFilter.h"
00005 #include "joint_tracker/RigidJointFilter.h"
00006 #include "joint_tracker/PrismaticJointFilter.h"
00007 #include "joint_tracker/RevoluteJointFilter.h"
00008 #include "joint_tracker/DisconnectedJointFilter.h"
00009
00010 using namespace omip;
00011 using namespace MatrixWrapper;
00012 using namespace BFL;
00013
00014 JointCombinedFilterId JointCombinedFilter::_joint_id_generator = 1;
00015
00016 JointCombinedFilter::JointCombinedFilter():
00017 _joint_id(JointCombinedFilter::_joint_id_generator++),
00018 _normalizing_term(0.25)
00019 {
00020 this->_joint_filters[DISCONNECTED_JOINT] = DisconnectedJointFilterPtr( new DisconnectedJointFilter());
00021 this->_joint_filters[RIGID_JOINT] = RigidJointFilterPtr(new RigidJointFilter());
00022 this->_joint_filters[PRISMATIC_JOINT] = PrismaticJointFilterPtr( new PrismaticJointFilter());
00023 this->_joint_filters[REVOLUTE_JOINT] = RevoluteJointFilterPtr( new RevoluteJointFilter());
00024
00025 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00026 {
00027 joint_filter_it.second->setJointId(_joint_id);
00028 }
00029
00030 this->_joint_normalized_prediction_errors[RIGID_JOINT] = -1.;
00031 this->_joint_normalized_prediction_errors[PRISMATIC_JOINT] = -1.;
00032 this->_joint_normalized_prediction_errors[REVOLUTE_JOINT] = -1.;
00033 this->_joint_normalized_prediction_errors[DISCONNECTED_JOINT] = -1.;
00034 }
00035
00036 void JointCombinedFilter::setJointLikelihoodDisconnected(double disconnected_joint_likelihood)
00037 {
00038 boost::static_pointer_cast<DisconnectedJointFilter>(this->_joint_filters[DISCONNECTED_JOINT])->setLikelihoodOfLastMeasurements(disconnected_joint_likelihood);
00039 }
00040
00041 void JointCombinedFilter::setLoopPeriodNS(double loop_period_ns)
00042 {
00043 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00044 {
00045 joint_filter_it.second->setLoopPeriodNS(loop_period_ns);
00046 }
00047 }
00048
00049 void JointCombinedFilter::setNumSamplesForLikelihoodEstimation(int likelihood_sample_num)
00050 {
00051 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00052 {
00053 joint_filter_it.second->setNumSamplesForLikelihoodEstimation(likelihood_sample_num);
00054 }
00055 }
00056
00057 void JointCombinedFilter::setNormalizingTerm(double normalizing_term)
00058 {
00059 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00060 {
00061 joint_filter_it.second->setNormalizingTerm(normalizing_term);
00062 }
00063 }
00064
00065 void JointCombinedFilter::setCovarianceDeltaMeasurementLinear(double sigma_delta_meas_uncertainty_linear)
00066 {
00067 boost::static_pointer_cast<PrismaticJointFilter>(this->_joint_filters[PRISMATIC_JOINT])->setCovarianceDeltaMeasurementLinear(sigma_delta_meas_uncertainty_linear);
00068 }
00069
00070 void JointCombinedFilter::setCovarianceDeltaMeasurementAngular(double sigma_delta_meas_uncertainty_angular)
00071 {
00072 boost::static_pointer_cast<RevoluteJointFilter>(this->_joint_filters[REVOLUTE_JOINT])->setCovarianceDeltaMeasurementAngular(sigma_delta_meas_uncertainty_angular);
00073 }
00074
00075 void JointCombinedFilter::initialize()
00076 {
00077 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00078 {
00079 joint_filter_it.second->initialize();
00080 }
00081 }
00082
00083 void JointCombinedFilter::setMaxTranslationRigid(double rig_max_translation)
00084 {
00085 boost::static_pointer_cast<RigidJointFilter>(this->_joint_filters[RIGID_JOINT])->setMaxTranslationRigid(rig_max_translation);
00086 }
00087
00088 void JointCombinedFilter::setMaxRotationRigid(double rig_max_rotation)
00089 {
00090 boost::static_pointer_cast<RigidJointFilter>(this->_joint_filters[RIGID_JOINT])->setMaxRotationRigid(rig_max_rotation);
00091 }
00092
00093 void JointCombinedFilter::setMinRotationRevolute(const double& value)
00094 {
00095 boost::static_pointer_cast<RevoluteJointFilter>(this->_joint_filters[REVOLUTE_JOINT])->setMinRotationRevolute(value);
00096 }
00097
00098 void JointCombinedFilter::setMaxRadiusDistanceRevolute(const double& value)
00099 {
00100 boost::static_pointer_cast<RevoluteJointFilter>(this->_joint_filters[REVOLUTE_JOINT])->setMaxRadiusDistanceRevolute(value);
00101 }
00102
00103 void JointCombinedFilter::setCovariancePrior(const JointFilterType& joint_type, double prior_cov_vel)
00104 {
00105 this->_joint_filters[joint_type]->setCovariancePrior(prior_cov_vel);
00106 }
00107
00108 void JointCombinedFilter::setCovarianceAdditiveSystemNoisePhi(const JointFilterType& joint_type, double sys_noise_phi)
00109 {
00110 this->_joint_filters[joint_type]->setCovarianceAdditiveSystemNoisePhi(sys_noise_phi);
00111 }
00112
00113 void JointCombinedFilter::setCovarianceAdditiveSystemNoiseTheta(const JointFilterType& joint_type, double sys_noise_theta)
00114 {
00115 this->_joint_filters[joint_type]->setCovarianceAdditiveSystemNoiseTheta(sys_noise_theta);
00116 }
00117
00118 void JointCombinedFilter::setCovarianceAdditiveSystemNoisePx(const JointFilterType& joint_type, double sys_noise_px)
00119 {
00120 this->_joint_filters[joint_type]->setCovarianceAdditiveSystemNoisePx(sys_noise_px);
00121 }
00122
00123 void JointCombinedFilter::setCovarianceAdditiveSystemNoisePy(const JointFilterType& joint_type, double sys_noise_py)
00124 {
00125 this->_joint_filters[joint_type]->setCovarianceAdditiveSystemNoisePy(sys_noise_py);
00126 }
00127
00128 void JointCombinedFilter::setCovarianceAdditiveSystemNoisePz(const JointFilterType& joint_type, double sys_noise_pz)
00129 {
00130 this->_joint_filters[joint_type]->setCovarianceAdditiveSystemNoisePz(sys_noise_pz);
00131 }
00132
00133 void JointCombinedFilter::setCovarianceAdditiveSystemNoiseJointState(const JointFilterType& joint_type, double sys_noise_js)
00134 {
00135 this->_joint_filters[joint_type]->setCovarianceAdditiveSystemNoiseJointState(sys_noise_js);
00136 }
00137
00138 void JointCombinedFilter::setCovarianceAdditiveSystemNoiseJointVelocity(const JointFilterType& joint_type, double sys_noise_jv)
00139 {
00140 this->_joint_filters[joint_type]->setCovarianceAdditiveSystemNoiseJointVelocity(sys_noise_jv);
00141 }
00142
00143 void JointCombinedFilter::setCovarianceMeasurementNoise(const JointFilterType& joint_type, double meas_noise)
00144 {
00145 this->_joint_filters[joint_type]->setCovarianceMeasurementNoise(meas_noise);
00146 }
00147
00148 void JointCombinedFilter::setInitialMeasurement(const joint_measurement_t &initial_measurement,
00149 const Eigen::Twistd& rrb_pose_at_srb_birth_in_sf,
00150 const Eigen::Twistd& srb_pose_at_srb_birth_in_sf)
00151 {
00152 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00153 {
00154 joint_filter_it.second->setInitialMeasurement(initial_measurement,
00155 rrb_pose_at_srb_birth_in_sf,
00156 srb_pose_at_srb_birth_in_sf);
00157 }
00158 }
00159
00160 JointCombinedFilter::~JointCombinedFilter()
00161 {
00162
00163 }
00164
00165 JointCombinedFilter::JointCombinedFilter(const JointCombinedFilter &joint)
00166 {
00167 this->_joint_id = joint._joint_id;
00168 this->_joint_filters = joint._joint_filters;
00169 this->_joint_normalized_prediction_errors = joint._joint_normalized_prediction_errors;
00170 this->_normalizing_term = joint._normalizing_term;
00171 }
00172
00173 void JointCombinedFilter::predictMeasurement()
00174 {
00175 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00176 {
00177 joint_filter_it.second->predictMeasurement();
00178 }
00179 }
00180
00181 std::map<JointFilterType, Eigen::Twistd> JointCombinedFilter::getPredictedMeasurement()
00182 {
00183 std::map<JointFilterType, Eigen::Twistd> second_rb_hypotheses;
00184 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00185 {
00186 second_rb_hypotheses[joint_filter_it.first] = joint_filter_it.second->getPredictedMeasurement();
00187 }
00188 return second_rb_hypotheses;
00189 }
00190
00191 void JointCombinedFilter::setMeasurement(joint_measurement_t acquired_measurement, const double &measurement_timestamp_ns)
00192 {
00193 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00194 {
00195 joint_filter_it.second->setMeasurement(acquired_measurement, measurement_timestamp_ns);
00196 }
00197 }
00198
00199 void JointCombinedFilter::correctState()
00200 {
00201 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00202 {
00203 joint_filter_it.second->correctState();
00204 }
00205 }
00206
00207 void JointCombinedFilter::predictState(double time_interval_ns)
00208 {
00209 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00210 {
00211 joint_filter_it.second->predictState(time_interval_ns);
00212 }
00213 }
00214
00215 void JointCombinedFilter::normalizeJointFilterProbabilities()
00216 {
00217 this->_normalizing_term = 0.0;
00218 double sum_of_unnormalized_model_probabilities = 0.0;
00219
00220
00221
00222 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00223 {
00224
00225
00226 sum_of_unnormalized_model_probabilities += joint_filter_it.second->getUnnormalizedProbabilityOfJointFilter();
00227
00228 }
00229
00230 double normalized_disconnected_probability = this->_joint_filters[DISCONNECTED_JOINT]->getProbabilityOfJointFilter();
00231 this->_normalizing_term =
00232 (sum_of_unnormalized_model_probabilities);
00233
00234
00235 if(_normalizing_term < 1e-5)
00236 {
00237 _normalizing_term = 1e-5;
00238 }
00239
00240 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00241 {
00242 joint_filter_it.second->setNormalizingTerm(this->_normalizing_term);
00243 }
00244 }
00245
00246 void JointCombinedFilter::estimateJointFilterProbabilities()
00247 {
00248 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00249 {
00250 joint_filter_it.second->estimateMeasurementHistoryLikelihood();
00251 joint_filter_it.second->estimateUnnormalizedModelProbability();
00252 }
00253 this->normalizeJointFilterProbabilities();
00254 }
00255
00256 JointFilterPtr JointCombinedFilter::getMostProbableJointFilter()
00257 {
00258 double max_probability_of_a_model = 0.0;
00259 JointFilterPtr most_probable_joint_type = this->_joint_filters.at(DISCONNECTED_JOINT);
00260 BOOST_FOREACH(joint_filters_map::value_type joint_filter_it, this->_joint_filters)
00261 {
00262 double probability_of_a_model = joint_filter_it.second->getProbabilityOfJointFilter();
00263 if (probability_of_a_model > max_probability_of_a_model)
00264 {
00265 max_probability_of_a_model = probability_of_a_model;
00266 most_probable_joint_type = this->_joint_filters.at(joint_filter_it.first);
00267 }
00268 }
00269 return most_probable_joint_type;
00270 }
00271
00272 JointFilterPtr JointCombinedFilter::getJointFilter(JointFilterType joint_type) const
00273 {
00274 return this->_joint_filters.at(joint_type);
00275 }
00276
00277 JointCombinedFilterId JointCombinedFilter::getJointCombinedFilterId() const
00278 {
00279 return this->_joint_id;
00280 }
00281
00282 void JointCombinedFilter::setJointCombinedFilterId(JointCombinedFilterId new_joint_combined_filter_id)
00283 {
00284 this->_joint_id = new_joint_combined_filter_id;
00285 }
00286