gtsam 4.2.0
gtsam
BarometricFactor.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
18#pragma once
19
23
24namespace gtsam {
25
34class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
35 private:
37
38 double nT_;
39
40 public:
42 typedef boost::shared_ptr<BarometricFactor> shared_ptr;
43
46
48 BarometricFactor() : nT_(0) {}
49
50 ~BarometricFactor() override {}
51
59 BarometricFactor(Key key, Key baroKey, const double& baroIn,
60 const SharedNoiseModel& model)
61 : Base(model, key, baroKey), nT_(heightOut(baroIn)) {}
62
64 gtsam::NonlinearFactor::shared_ptr clone() const override {
65 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
66 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
67 }
68
70 void print(
71 const std::string& s = "",
72 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
73
75 bool equals(const NonlinearFactor& expected,
76 double tol = 1e-9) const override;
77
79 Vector evaluateError(
80 const Pose3& p, const double& b,
81 boost::optional<Matrix&> H = boost::none,
82 boost::optional<Matrix&> H2 = boost::none) const override;
83
84 inline const double& measurementIn() const { return nT_; }
85
86 inline double heightOut(double n) const {
87 // From https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html
88 return (std::pow(n / 101.29, 1. / 5.256) * 288.08 - 273.1 - 15.04) /
89 -0.00649;
90 };
91
92 inline double baroOut(const double& meters) {
93 double temp = 15.04 - 0.00649 * meters;
94 return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256);
95 };
96
97 private:
99 friend class boost::serialization::access;
100 template <class ARCHIVE>
101 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
102 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
103 ar& boost::serialization::make_nvp(
104 "NoiseModelFactor1",
105 boost::serialization::base_object<Base>(*this));
106 ar& BOOST_SERIALIZATION_NVP(nT_);
107 }
108};
109
110} // namespace gtsam
3D Pose
Navigation state composing of attitude, position, and velocity.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition: NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
Template to create a binary predicate.
Definition: Testable.h:111
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
Definition: Factor.h:68
Prior on height in a cartesian frame.
Definition: BarometricFactor.h:34
BarometricFactor This
Typedef to this class.
Definition: BarometricFactor.h:45
BarometricFactor(Key key, Key baroKey, const double &baroIn, const SharedNoiseModel &model)
Constructor from a measurement of pressure in KPa.
Definition: BarometricFactor.h:59
boost::shared_ptr< BarometricFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: BarometricFactor.h:42
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: BarometricFactor.h:64
BarometricFactor()
default constructor - only use for serialization
Definition: BarometricFactor.h:48
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition: NonlinearFactor.h:400