Loading...
Searching...
No Matches
BarometricFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
private:
typedef NoiseModelFactorN<Pose3, double> Base;
double nT_;
public:
typedef boost::shared_ptr<BarometricFactor> shared_ptr;
typedef BarometricFactor This;
BarometricFactor() : nT_(0) {}
~BarometricFactor() override {}
BarometricFactor(Key key, Key baroKey, const double& baroIn,
const SharedNoiseModel& model)
: Base(model, key, baroKey), nT_(heightOut(baroIn)) {}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected,
double tol = 1e-9) const override;
Vector evaluateError(
const Pose3& p, const double& b,
boost::optional<Matrix&> H = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override;
inline const double& measurementIn() const { return nT_; }
inline double heightOut(double n) const {
// From https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html
return (std::pow(n / 101.29, 1. / 5.256) * 288.08 - 273.1 - 15.04) /
-0.00649;
};
inline double baroOut(const double& meters) {
double temp = 15.04 - 0.00649 * meters;
return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256);
};
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar& boost::serialization::make_nvp(
"NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(nT_);
}
};
} // namespace gtsam
Pose3.h
3D Pose
NavState.h
Navigation state composing of attitude, position, and velocity.
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::KeyFormatter
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
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::BarometricFactor
Prior on height in a cartesian frame.
Definition BarometricFactor.h:34
gtsam::BarometricFactor::This
BarometricFactor This
Typedef to this class.
Definition BarometricFactor.h:45
gtsam::BarometricFactor::BarometricFactor
BarometricFactor(Key key, Key baroKey, const double &baroIn, const SharedNoiseModel &model)
Constructor from a measurement of pressure in KPa.
Definition BarometricFactor.h:59
gtsam::BarometricFactor::shared_ptr
boost::shared_ptr< BarometricFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition BarometricFactor.h:42
gtsam::BarometricFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition BarometricFactor.h:64
gtsam::BarometricFactor::BarometricFactor
BarometricFactor()
default constructor - only use for serialization
Definition BarometricFactor.h:48
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactorN< Pose3, double >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN< Pose3, double >::key
Key key() const
Definition NonlinearFactor.h:518