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 #ifndef HECTOR_POSE_ESTIMATION_POSEUPDATE_H
00030 #define HECTOR_POSE_ESTIMATION_POSEUPDATE_H
00031
00032 #include <hector_pose_estimation/measurement.h>
00033 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00034 #include <geometry_msgs/TwistWithCovarianceStamped.h>
00035
00036 #include <boost/function.hpp>
00037
00038 namespace hector_pose_estimation {
00039
00040 class PositionXYModel : public MeasurementModel_<PositionXYModel,2> {
00041 public:
00042 PositionXYModel() {}
00043 virtual ~PositionXYModel() {}
00044
00045 virtual void getExpectedValue(MeasurementVector& y_pred, const State& state);
00046 virtual void getStateJacobian(MeasurementMatrix& C, const State& state, bool init);
00047
00048 void updateState(State& state, const ColumnVector &diff) const;
00049 };
00050
00051 class PositionZModel : public MeasurementModel_<PositionZModel,1> {
00052 public:
00053 PositionZModel() {}
00054 virtual ~PositionZModel() {}
00055
00056 virtual void getExpectedValue(MeasurementVector& y_pred, const State& state);
00057 virtual void getStateJacobian(MeasurementMatrix& C, const State& state, bool init);
00058
00059 void updateState(State& state, const ColumnVector &diff) const;
00060 };
00061
00062 class YawModel : public MeasurementModel_<YawModel,1> {
00063 public:
00064 YawModel() {}
00065 virtual ~YawModel() {}
00066
00067 virtual void getExpectedValue(MeasurementVector& y_pred, const State& state);
00068 virtual void getStateJacobian(MeasurementMatrix& C, const State& state, bool init);
00069
00070 void updateState(State& state, const ColumnVector &diff) const;
00071 };
00072
00073 class TwistModel : public MeasurementModel_<TwistModel,6> {
00074 public:
00075 TwistModel() {}
00076 virtual ~TwistModel() {}
00077
00078 virtual void getExpectedValue(MeasurementVector& y_pred, const State& state);
00079 virtual void getStateJacobian(MeasurementMatrix& C, const State& state, bool init);
00080 };
00081
00082 class PoseUpdate : public Measurement
00083 {
00084 public:
00085 PoseUpdate(const std::string& name = "poseupdate");
00086 virtual ~PoseUpdate();
00087
00088 class Update : public MeasurementUpdate {
00089 public:
00090 Update() {}
00091 Update(const geometry_msgs::PoseWithCovarianceStamped& pose) : pose(new geometry_msgs::PoseWithCovarianceStamped(pose)) {}
00092 Update(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose) : pose(pose) {}
00093 Update(const geometry_msgs::TwistWithCovarianceStamped& twist) : twist(new geometry_msgs::TwistWithCovarianceStamped(twist)) {}
00094 Update(const geometry_msgs::TwistWithCovarianceStampedConstPtr& twist) : twist(twist) {}
00095 Update(const geometry_msgs::PoseWithCovarianceStamped& pose, const geometry_msgs::TwistWithCovarianceStamped& twist) : pose(new geometry_msgs::PoseWithCovarianceStamped(pose)), twist(new geometry_msgs::TwistWithCovarianceStamped(twist)) {}
00096 Update(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose, const geometry_msgs::TwistWithCovarianceStampedConstPtr& twist) : pose(pose), twist(twist) {}
00097 geometry_msgs::PoseWithCovarianceStampedConstPtr pose;
00098 geometry_msgs::TwistWithCovarianceStampedConstPtr twist;
00099 };
00100
00101 virtual bool updateImpl(const MeasurementUpdate &update);
00102
00103 private:
00104 PositionXYModel position_xy_model_;
00105 PositionZModel position_z_model_;
00106 YawModel yaw_model_;
00107 TwistModel twist_model_;
00108
00109 double fixed_alpha_, fixed_beta_;
00110 bool interpret_covariance_as_information_matrix_;
00111 double max_time_difference_;
00112 bool predict_pose_;
00113 bool jump_on_max_error_;
00114
00115 double fixed_position_xy_stddev_;
00116 double fixed_position_z_stddev_;
00117 double fixed_yaw_stddev_;
00118
00119 double fixed_velocity_xy_stddev_;
00120 double fixed_velocity_z_stddev_;
00121 double fixed_angular_rate_xy_stddev_;
00122 double fixed_angular_rate_z_stddev_;
00123
00124 double max_position_xy_error_;
00125 double max_position_z_error_;
00126 double max_yaw_error_;
00127
00128 double max_velocity_xy_error_;
00129 double max_velocity_z_error_;
00130 double max_angular_rate_xy_error_;
00131 double max_angular_rate_z_error_;
00132
00133 typedef boost::function<void(State &state, const ColumnVector &diff)> JumpFunction;
00134 double calculateOmega(const SymmetricMatrix &Ix, const SymmetricMatrix &Iy);
00135
00136 template <typename MeasurementVector, typename MeasurementMatrix, typename NoiseVariance>
00137 double updateInternal(State &state, const NoiseVariance &Iy, const MeasurementVector &error, const MeasurementMatrix &H, const std::string& text, const double max_error = 0.0, JumpFunction jump_function = JumpFunction());
00138
00139 protected:
00140 Queue_<Update> queue_;
00141 virtual Queue& queue() { return queue_; }
00142 };
00143
00144 }
00145
00146 #endif // HECTOR_POSE_ESTIMATION_POSEUPDATE_H