Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef JOINTCOMBINEDFILTER_H_
00033 #define JOINTCOMBINEDFILTER_H_
00034
00035 #include <pcl/point_types.h>
00036 #include <unsupported/Eigen/MatrixFunctions>
00037 #include <Eigen/Geometry>
00038 #include <lgsm/Lgsm>
00039 #include <boost/shared_ptr.hpp>
00040 #include <boost/unordered_map.hpp>
00041
00042 #include <ros/ros.h>
00043 #include <geometry_msgs/Pose.h>
00044 #include <geometry_msgs/Twist.h>
00045 #include <geometry_msgs/PoseWithCovariance.h>
00046 #include <tf/tf.h>
00047 #include <tf/transform_datatypes.h>
00048 #include <tf/transform_broadcaster.h>
00049 #include <tf_conversions/tf_eigen.h>
00050
00051 #include "joint_tracker/JointFilter.h"
00052 #include "omip_common/OMIPTypeDefs.h"
00053
00054 namespace omip
00055 {
00056
00057 class JointCombinedFilter;
00058 typedef boost::shared_ptr<JointCombinedFilter> JointCombinedFilterPtr;
00059
00060 typedef std::map<JointFilterType, JointFilterPtr> joint_filters_map;
00061
00070 class JointCombinedFilter
00071 {
00072 public:
00073
00077 JointCombinedFilter();
00078
00079 void setJointCombinedFilterId(JointCombinedFilterId new_joint_combined_filter_id);
00080
00084 virtual ~JointCombinedFilter();
00085
00092 JointCombinedFilterPtr clone() const
00093 {
00094 return (JointCombinedFilterPtr(doClone()));
00095 }
00096
00100 JointCombinedFilter(const JointCombinedFilter &joint_combined_filter);
00101
00106 virtual void predictState(double time_interval_ns);
00107
00112 virtual void predictMeasurement();
00113
00118 virtual void correctState();
00119
00125 virtual void setMeasurement(joint_measurement_t acquired_measurement, const double& measurement_timestamp_ns);
00126
00131 std::map<JointFilterType, Eigen::Twistd> getPredictedMeasurement();
00132
00133 virtual void estimateJointFilterProbabilities();
00134
00147 virtual void normalizeJointFilterProbabilities();
00148
00153 virtual JointFilterPtr getMostProbableJointFilter();
00154
00161 virtual JointFilterPtr getJointFilter(JointFilterType joint_type) const;
00162
00166 virtual JointCombinedFilterId getJointCombinedFilterId() const;
00167
00168 virtual void setJointLikelihoodDisconnected(double disconnected_joint_likelihood);
00169
00170 virtual void setLoopPeriodNS(double loop_period_ns);
00171
00172 virtual void setNumSamplesForLikelihoodEstimation(int likelihood_sample_num);
00173
00174 virtual void setNormalizingTerm(double normalizing_term);
00175
00176 virtual void setCovarianceDeltaMeasurementLinear(double sigma_delta_meas_uncertainty_linear);
00177
00178 virtual void setCovarianceDeltaMeasurementAngular(double sigma_delta_meas_uncertainty_angular);
00179
00180 virtual void initialize();
00181
00182 virtual void setMaxTranslationRigid(double rig_max_translation);
00183
00184 virtual void setMaxRotationRigid(double rig_max_rotation);
00185
00186 virtual void setCovariancePrior(const JointFilterType& joint_type, double prior_cov_vel);
00187
00188 virtual void setCovarianceAdditiveSystemNoisePhi(const JointFilterType& joint_type, double sys_noise_phi);
00189
00190 virtual void setCovarianceAdditiveSystemNoiseTheta(const JointFilterType& joint_type, double sys_noise_theta);
00191
00192 virtual void setCovarianceAdditiveSystemNoisePx(const JointFilterType& joint_type, double sys_noise_px);
00193
00194 virtual void setCovarianceAdditiveSystemNoisePy(const JointFilterType& joint_type, double sys_noise_py);
00195
00196 virtual void setCovarianceAdditiveSystemNoisePz(const JointFilterType& joint_type, double sys_noise_pz);
00197
00198 virtual void setCovarianceAdditiveSystemNoiseJointState(const JointFilterType& joint_type, double sys_noise_js);
00199
00200 virtual void setCovarianceAdditiveSystemNoiseJointVelocity(const JointFilterType& joint_type, double sys_noise_jv);
00201
00202 virtual void setCovarianceMeasurementNoise(const JointFilterType& joint_type, double meas_noise);
00203
00204 virtual void setMinRotationRevolute(const double& value);
00205
00206 virtual void setMaxRadiusDistanceRevolute(const double& value);
00207
00208 virtual void setInitialMeasurement(const joint_measurement_t &initial_measurement,
00209 const Eigen::Twistd& rrb_pose_at_srb_birth_in_sf, const Eigen::Twistd& srb_pose_at_srb_birth_in_sf);
00210
00211 protected:
00212
00213 static JointCombinedFilterId _joint_id_generator;
00214
00215 JointCombinedFilterId _joint_id;
00216
00217 double _normalizing_term;
00218
00219
00220 joint_filters_map _joint_filters;
00221
00222
00223 std::map<JointFilterType, double> _joint_normalized_prediction_errors;
00224
00225 virtual JointCombinedFilter* doClone() const
00226 {
00227 return (new JointCombinedFilter(*this));
00228 }
00229
00230 };
00231
00232 }
00233
00234 #endif