odom_estimation.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * notice, this list of conditions and the following disclaimer.
12 * * Redistributions in binary form must reproduce the above
13 * copyright notice, this list of conditions and the following
14 * disclaimer in the documentation and/or other materials provided
15 * with the distribution.
16 * * Neither the name of the Willow Garage nor the names of its
17 * contributors may be used to endorse or promote products derived
18 * from this software without specific prior written permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 * POSSIBILITY OF SUCH DAMAGE.
32 *********************************************************************/
33 
34 /* Author: Wim Meeussen */
35 
36 #ifndef __ODOM_ESTIMATION__
37 #define __ODOM_ESTIMATION__
38 
39 // bayesian filtering
40 #include <bfl/filter/extendedkalmanfilter.h>
41 #include <bfl/wrappers/matrix/matrix_wrapper.h>
42 #include <bfl/model/linearanalyticsystemmodel_gaussianuncertainty.h>
43 #include <bfl/model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
44 #include <bfl/pdf/analyticconditionalgaussian.h>
45 #include <bfl/pdf/linearanalyticconditionalgaussian.h>
47 
48 // TF
49 #include <tf/tf.h>
50 
51 // msgs
52 #include <geometry_msgs/PoseWithCovarianceStamped.h>
53 
54 // log files
55 #include <fstream>
56 
57 namespace estimation
58 {
59 
61 {
62 public:
65 
67  virtual ~OdomEstimation();
68 
78  bool update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const ros::Time& filter_time, bool& diagnostics_res);
79 
84  void initialize(const tf::Transform& prior, const ros::Time& time);
85 
90 
94  void getEstimate(MatrixWrapper::ColumnVector& estimate);
95 
100  void getEstimate(ros::Time time, tf::Transform& estimate);
101 
106  void getEstimate(ros::Time time, tf::StampedTransform& estimate);
107 
111  void getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate);
112 
116  void addMeasurement(const tf::StampedTransform& meas);
117 
122  void addMeasurement(const tf::StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar);
123 
127  void setOutputFrame(const std::string& output_frame);
128 
132  void setBaseFootprintFrame(const std::string& base_frame);
133 
134 private:
136  void angleOverflowCorrect(double& a, double ref);
137 
138  // decompose Transform into x,y,z,Rx,Ry,Rz
139  void decomposeTransform(const tf::StampedTransform& trans,
140  double& x, double& y, double&z, double&Rx, double& Ry, double& Rz);
141  void decomposeTransform(const tf::Transform& trans,
142  double& x, double& y, double&z, double&Rx, double& Ry, double& Rz);
143 
144 
145  // pdf / model / filter
159 
160  // vars
161  MatrixWrapper::ColumnVector vel_desi_, filter_estimate_old_vec_;
166 
167  // diagnostics
169 
170  // tf transformer
172 
173  std::string output_frame_;
175 
176 }; // class
177 
178 }; // namespace
179 
180 #endif
MatrixWrapper::ColumnVector filter_estimate_old_vec_
tf::StampedTransform odom_meas_old_
virtual ~OdomEstimation()
destructor
tf::StampedTransform imu_meas_old_
BFL::LinearAnalyticMeasurementModelGaussianUncertainty * odom_meas_model_
void angleOverflowCorrect(double &a, double ref)
correct for angle overflow
tf::StampedTransform gps_meas_old_
tf::StampedTransform vo_meas_old_
void initialize(const tf::Transform &prior, const ros::Time &time)
BFL::LinearAnalyticMeasurementModelGaussianUncertainty * vo_meas_model_
MatrixWrapper::SymmetricMatrix odom_covariance_
BFL::LinearAnalyticConditionalGaussian * vo_meas_pdf_
tf::StampedTransform vo_meas_
void decomposeTransform(const tf::StampedTransform &trans, double &x, double &y, double &z, double &Rx, double &Ry, double &Rz)
BFL::AnalyticSystemModelGaussianUncertainty * sys_model_
BFL::LinearAnalyticConditionalGaussian * gps_meas_pdf_
BFL::LinearAnalyticMeasurementModelGaussianUncertainty * gps_meas_model_
BFL::LinearAnalyticMeasurementModelGaussianUncertainty * imu_meas_model_
MatrixWrapper::SymmetricMatrix vo_covariance_
MatrixWrapper::SymmetricMatrix imu_covariance_
bool update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const ros::Time &filter_time, bool &diagnostics_res)
tf::StampedTransform odom_meas_
void addMeasurement(const tf::StampedTransform &meas)
tf::Transform filter_estimate_old_
BFL::LinearAnalyticConditionalGaussian * imu_meas_pdf_
BFL::ExtendedKalmanFilter * filter_
MatrixWrapper::ColumnVector vel_desi_
tf::StampedTransform gps_meas_
BFL::LinearAnalyticConditionalGaussian * odom_meas_pdf_
void setBaseFootprintFrame(const std::string &base_frame)
MatrixWrapper::SymmetricMatrix gps_covariance_
BFL::NonLinearAnalyticConditionalGaussianOdo * sys_pdf_
void setOutputFrame(const std::string &output_frame)
void getEstimate(MatrixWrapper::ColumnVector &estimate)
tf::StampedTransform imu_meas_


robot_pose_ekf
Author(s): Wim Meeussen
autogenerated on Mon Feb 28 2022 23:26:03