poseupdate.h
Go to the documentation of this file.
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 #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 
00112   double fixed_position_xy_stddev_;
00113   double fixed_position_z_stddev_;
00114   double fixed_yaw_stddev_;
00115 
00116   double fixed_velocity_xy_stddev_;
00117   double fixed_velocity_z_stddev_;
00118   double fixed_angular_rate_xy_stddev_;
00119   double fixed_angular_rate_z_stddev_;
00120 
00121   double max_time_difference_;
00122   double max_position_xy_error_;
00123   double max_position_z_error_;
00124   double max_yaw_error_;
00125 
00126   double max_velocity_xy_error_;
00127   double max_velocity_z_error_;
00128   double max_angular_rate_xy_error_;
00129   double max_angular_rate_z_error_;
00130 
00131   bool jump_on_max_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 } // namespace hector_pose_estimation
00145 
00146 #endif // HECTOR_POSE_ESTIMATION_POSEUPDATE_H


hector_pose_estimation_core
Author(s): Johannes Meyer
autogenerated on Mon Oct 6 2014 00:24:16