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 {
00041 public:
00042 static const unsigned int MeasurementDimension = 2;
00043 typedef ColumnVector_<MeasurementDimension> MeasurementVector;
00044 typedef SymmetricMatrix_<MeasurementDimension> NoiseCovariance;
00045
00046 PositionXYModel() : MeasurementModel(MeasurementDimension) {}
00047 virtual ~PositionXYModel() {}
00048
00049 virtual ColumnVector ExpectedValueGet() const;
00050 virtual Matrix dfGet(unsigned int i) const;
00051
00052 void updateState(const SymmetricMatrix &Px, const ColumnVector &x, const ColumnVector &diff, SymmetricMatrix &Pxy, ColumnVector &xy) const;
00053 };
00054
00055 class PositionZModel : public MeasurementModel {
00056 public:
00057 static const unsigned int MeasurementDimension = 1;
00058 typedef ColumnVector_<MeasurementDimension> MeasurementVector;
00059 typedef SymmetricMatrix_<MeasurementDimension> NoiseCovariance;
00060
00061 PositionZModel() : MeasurementModel(MeasurementDimension) {}
00062 virtual ~PositionZModel() {}
00063
00064 virtual ColumnVector ExpectedValueGet() const;
00065 virtual Matrix dfGet(unsigned int i) const;
00066
00067 void updateState(const SymmetricMatrix &Px, const ColumnVector &x, const ColumnVector &diff, SymmetricMatrix &Pxy, ColumnVector &xy) const;
00068 };
00069
00070 class YawModel : public MeasurementModel {
00071 public:
00072 static const unsigned int MeasurementDimension = 1;
00073 typedef ColumnVector_<MeasurementDimension> MeasurementVector;
00074 typedef SymmetricMatrix_<MeasurementDimension> NoiseCovariance;
00075
00076 YawModel() : MeasurementModel(MeasurementDimension) {}
00077 virtual ~YawModel() {}
00078
00079 virtual ColumnVector ExpectedValueGet() const;
00080 virtual Matrix dfGet(unsigned int i) const;
00081
00082 void updateState(const SymmetricMatrix &Px, const ColumnVector &x, const ColumnVector &diff, SymmetricMatrix &Pxy, ColumnVector &xy) const;
00083 };
00084
00085 class TwistModel : public MeasurementModel {
00086 public:
00087 static const unsigned int MeasurementDimension = 6;
00088 typedef ColumnVector_<MeasurementDimension> MeasurementVector;
00089 typedef SymmetricMatrix_<MeasurementDimension> NoiseCovariance;
00090
00091 TwistModel() : MeasurementModel(MeasurementDimension) {}
00092 virtual ~TwistModel() {}
00093
00094 virtual ColumnVector ExpectedValueGet() const;
00095 virtual Matrix dfGet(unsigned int i) const;
00096 };
00097
00098 class PoseUpdate : public Measurement
00099 {
00100 public:
00101 PoseUpdate(const std::string& name = "poseupdate");
00102 virtual ~PoseUpdate();
00103
00104 class Update : public MeasurementUpdate {
00105 public:
00106 Update() {}
00107 Update(const geometry_msgs::PoseWithCovarianceStamped& pose) : pose(new geometry_msgs::PoseWithCovarianceStamped(pose)) {}
00108 Update(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose) : pose(pose) {}
00109 Update(const geometry_msgs::TwistWithCovarianceStamped& twist) : twist(new geometry_msgs::TwistWithCovarianceStamped(twist)) {}
00110 Update(const geometry_msgs::TwistWithCovarianceStampedConstPtr& twist) : twist(twist) {}
00111 Update(const geometry_msgs::PoseWithCovarianceStamped& pose, const geometry_msgs::TwistWithCovarianceStamped& twist) : pose(new geometry_msgs::PoseWithCovarianceStamped(pose)), twist(new geometry_msgs::TwistWithCovarianceStamped(twist)) {}
00112 Update(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose, const geometry_msgs::TwistWithCovarianceStampedConstPtr& twist) : pose(pose), twist(twist) {}
00113 geometry_msgs::PoseWithCovarianceStampedConstPtr pose;
00114 geometry_msgs::TwistWithCovarianceStampedConstPtr twist;
00115 };
00116
00117 bool update(PoseEstimation &estimator, const MeasurementUpdate &update);
00118
00119 private:
00120 PositionXYModel position_xy_model_;
00121 PositionZModel position_z_model_;
00122 YawModel yaw_model_;
00123 TwistModel twist_model_;
00124
00125 double fixed_alpha_, fixed_beta_;
00126 bool interpret_covariance_as_information_matrix_;
00127
00128 double fixed_position_xy_stddev_;
00129 double fixed_position_z_stddev_;
00130 double fixed_yaw_stddev_;
00131
00132 double fixed_velocity_xy_stddev_;
00133 double fixed_velocity_z_stddev_;
00134 double fixed_angular_rate_xy_stddev_;
00135 double fixed_angular_rate_z_stddev_;
00136
00137 double max_time_difference_;
00138 double max_position_xy_error_;
00139 double max_position_z_error_;
00140 double max_yaw_error_;
00141
00142 double max_velocity_xy_error_;
00143 double max_velocity_z_error_;
00144 double max_angular_rate_xy_error_;
00145 double max_angular_rate_z_error_;
00146
00147 bool jump_on_max_error_;
00148
00149 typedef boost::function<void(const SymmetricMatrix &Px, const ColumnVector &x, const ColumnVector &diff, SymmetricMatrix &Pxy, ColumnVector &xy)> JumpFunction;
00150 double calculateOmega(const SymmetricMatrix &Ix, const SymmetricMatrix &Iy) const;
00151 double updateInternal(const SymmetricMatrix &Px, const ColumnVector &x, const SymmetricMatrix &Iy, const ColumnVector &error, const Matrix &H, SymmetricMatrix &Pxy, ColumnVector &xy, const std::string& text, const double max_error = 0.0, JumpFunction jump_function = JumpFunction());
00152
00153 protected:
00154 Queue_<Update> queue_;
00155 virtual Queue& queue() { return queue_; }
00156 };
00157
00158 }
00159
00160 #endif // HECTOR_POSE_ESTIMATION_POSEUPDATE_H