$search
00001 //================================================================================================= 00002 // Copyright (c) 2011, Johannes Meyer, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Flight Systems and Automatic Control group, 00013 // TU Darmstadt, nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 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 namespace hector_pose_estimation { 00037 00038 class PositionXYModel : public MeasurementModel { 00039 public: 00040 static const unsigned int MeasurementDimension = 2; 00041 typedef ColumnVector_<MeasurementDimension> MeasurementVector; 00042 typedef SymmetricMatrix_<MeasurementDimension> NoiseCovariance; 00043 00044 PositionXYModel() : MeasurementModel(MeasurementDimension) {} 00045 virtual ~PositionXYModel() {} 00046 00047 virtual ColumnVector ExpectedValueGet() const; 00048 virtual Matrix dfGet(unsigned int i) const; 00049 }; 00050 00051 class PositionZModel : public MeasurementModel { 00052 public: 00053 static const unsigned int MeasurementDimension = 1; 00054 typedef ColumnVector_<MeasurementDimension> MeasurementVector; 00055 typedef SymmetricMatrix_<MeasurementDimension> NoiseCovariance; 00056 00057 PositionZModel() : MeasurementModel(MeasurementDimension) {} 00058 virtual ~PositionZModel() {} 00059 00060 virtual ColumnVector ExpectedValueGet() const; 00061 virtual Matrix dfGet(unsigned int i) const; 00062 }; 00063 00064 class YawModel : public MeasurementModel { 00065 public: 00066 static const unsigned int MeasurementDimension = 1; 00067 typedef ColumnVector_<MeasurementDimension> MeasurementVector; 00068 typedef SymmetricMatrix_<MeasurementDimension> NoiseCovariance; 00069 00070 YawModel() : MeasurementModel(MeasurementDimension) {} 00071 virtual ~YawModel() {} 00072 00073 virtual ColumnVector ExpectedValueGet() const; 00074 virtual Matrix dfGet(unsigned int i) const; 00075 }; 00076 00077 class TwistModel : public MeasurementModel { 00078 public: 00079 static const unsigned int MeasurementDimension = 6; 00080 typedef ColumnVector_<MeasurementDimension> MeasurementVector; 00081 typedef SymmetricMatrix_<MeasurementDimension> NoiseCovariance; 00082 00083 TwistModel() : MeasurementModel(MeasurementDimension) {} 00084 virtual ~TwistModel() {} 00085 00086 virtual ColumnVector ExpectedValueGet() const; 00087 virtual Matrix dfGet(unsigned int i) const; 00088 }; 00089 00090 class PoseUpdate : public Measurement 00091 { 00092 public: 00093 PoseUpdate(const std::string& name = "poseupdate"); 00094 virtual ~PoseUpdate(); 00095 00096 class Update : public MeasurementUpdate { 00097 public: 00098 Update() {} 00099 Update(const geometry_msgs::PoseWithCovarianceStamped& pose) : pose(new geometry_msgs::PoseWithCovarianceStamped(pose)) {} 00100 Update(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose) : pose(pose) {} 00101 Update(const geometry_msgs::TwistWithCovarianceStamped& twist) : twist(new geometry_msgs::TwistWithCovarianceStamped(twist)) {} 00102 Update(const geometry_msgs::TwistWithCovarianceStampedConstPtr& twist) : twist(twist) {} 00103 Update(const geometry_msgs::PoseWithCovarianceStamped& pose, const geometry_msgs::TwistWithCovarianceStamped& twist) : pose(new geometry_msgs::PoseWithCovarianceStamped(pose)), twist(new geometry_msgs::TwistWithCovarianceStamped(twist)) {} 00104 Update(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose, const geometry_msgs::TwistWithCovarianceStampedConstPtr& twist) : pose(pose), twist(twist) {} 00105 geometry_msgs::PoseWithCovarianceStampedConstPtr pose; 00106 geometry_msgs::TwistWithCovarianceStampedConstPtr twist; 00107 }; 00108 00109 bool update(PoseEstimation &estimator, const MeasurementUpdate &update); 00110 00111 private: 00112 PositionXYModel position_xy_model_; 00113 PositionZModel position_z_model_; 00114 YawModel yaw_model_; 00115 TwistModel twist_model_; 00116 00117 double fixed_alpha_, fixed_beta_; 00118 bool interpret_covariance_as_information_matrix_; 00119 00120 double fixed_position_xy_stddev_; 00121 double fixed_position_z_stddev_; 00122 double fixed_yaw_stddev_; 00123 00124 double fixed_velocity_xy_stddev_; 00125 double fixed_velocity_z_stddev_; 00126 double fixed_angular_rate_xy_stddev_; 00127 double fixed_angular_rate_z_stddev_; 00128 00129 double max_time_difference_; 00130 double max_position_xy_error_; 00131 double max_position_z_error_; 00132 double max_yaw_error_; 00133 00134 double max_velocity_xy_error_; 00135 double max_velocity_z_error_; 00136 double max_angular_rate_xy_error_; 00137 double max_angular_rate_z_error_; 00138 00139 bool jump_on_max_error_; 00140 00141 double calculateOmega(const SymmetricMatrix &Ix, const SymmetricMatrix &Iy) const; 00142 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); 00143 00144 protected: 00145 Queue_<Update> queue_; 00146 virtual Queue& queue() { return queue_; } 00147 }; 00148 00149 } // namespace hector_pose_estimation 00150 00151 #endif // HECTOR_POSE_ESTIMATION_POSEUPDATE_H