diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 535d60eb1..a293c6ec2 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -15,7 +15,7 @@ set (gtsam_subdirs sam sfm slam - navigation + navigation ) set(gtsam_srcs) diff --git a/gtsam/navigation/BarometricFactor.cpp b/gtsam/navigation/BarometricFactor.cpp new file mode 100644 index 000000000..2f0ff7436 --- /dev/null +++ b/gtsam/navigation/BarometricFactor.cpp @@ -0,0 +1,55 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file BarometricFactor.cpp + * @author Peter Milani + * @brief Implementation file for Barometric factor + * @date December 16, 2021 + **/ + +#include "BarometricFactor.h" + +using namespace std; + +namespace gtsam { + +//*************************************************************************** +void BarometricFactor::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << (s.empty() ? "" : s + " ") << "Barometric Factor on " + << keyFormatter(key1()) << "Barometric Bias on " + << keyFormatter(key2()) << "\n"; + + cout << " Baro measurement: " << nT_ << "\n"; + noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool BarometricFactor::equals(const NonlinearFactor& expected, + double tol) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(nT_, e->nT_, tol); +} + +//*************************************************************************** +Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias, + boost::optional H, + boost::optional H2) const { + Matrix tH; + Vector ret = (Vector(1) << (p.translation(tH).z() + bias - nT_)).finished(); + if (H) (*H) = tH.block<1, 6>(2, 0); + if (H2) (*H2) = (Matrix(1, 1) << 1.0).finished(); + return ret; +} + +} // namespace gtsam diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h new file mode 100644 index 000000000..e7bf6f998 --- /dev/null +++ b/gtsam/navigation/BarometricFactor.h @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file BarometricFactor.h + * @author Peter Milani + * @brief Header file for Barometric factor + * @date December 16, 2021 + **/ +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Prior on height in a cartesian frame. + * Receive barometric pressure in kilopascals + * Model with a slowly moving bias to capture differences + * between the height and the standard atmosphere + * https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html + * @addtogroup Navigation + */ +class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2 { + private: + typedef NoiseModelFactor2 Base; + + double nT_; ///< Height Measurement based on a standard atmosphere + + public: + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Typedef to this class + typedef BarometricFactor This; + + /** default constructor - only use for serialization */ + BarometricFactor() : nT_(0) {} + + ~BarometricFactor() override {} + + /** + * @brief Constructor from a measurement of pressure in KPa. + * @param key of the Pose3 variable that will be constrained + * @param key of the barometric bias that will be constrained + * @param baroIn measurement in KPa + * @param model Gaussian noise model 1 dimension + */ + BarometricFactor(Key key, Key baroKey, const double& baroIn, + const SharedNoiseModel& model) + : Base(model, key, baroKey), nT_(heightOut(baroIn)) {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + + /// equals + bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const override; + + /// vector of errors + Vector evaluateError( + const Pose3& p, const double& b, + boost::optional H = boost::none, + boost::optional 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: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(nT_); + } +}; + +} // namespace gtsam diff --git a/gtsam/navigation/tests/testBarometricFactor.cpp b/gtsam/navigation/tests/testBarometricFactor.cpp new file mode 100644 index 000000000..47f4824c1 --- /dev/null +++ b/gtsam/navigation/tests/testBarometricFactor.cpp @@ -0,0 +1,129 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file testBarometricFactor.cpp + * @brief Unit test for BarometricFactor + * @author Peter Milani + * @date 16 Dec, 2021 + */ + +#include +#include +#include +#include + +#include + +using namespace std::placeholders; +using namespace std; +using namespace gtsam; + +// ************************************************************************* +namespace example {} + +double metersToBaro(const double& meters) { + double temp = 15.04 - 0.00649 * meters; + return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256); +} + +// ************************************************************************* +TEST(BarometricFactor, Constructor) { + using namespace example; + + // meters to barometric. + + double baroMeasurement = metersToBaro(10.); + + // Factor + Key key(1); + Key key2(2); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25); + BarometricFactor factor(key, key2, baroMeasurement, model); + + // Create a linearization point at zero error + Pose3 T(Rot3::RzRyRx(0., 0., 0.), Point3(0., 0., 10.)); + double baroBias = 0.; + Vector1 zero; + zero << 0.; + EXPECT(assert_equal(zero, factor.evaluateError(T, baroBias), 1e-5)); + + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative21( + std::bind(&BarometricFactor::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, boost::none, + boost::none), + T, baroBias); + + Matrix expectedH2 = numericalDerivative22( + std::bind(&BarometricFactor::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, boost::none, + boost::none), + T, baroBias); + + // Use the factor to calculate the derivative + Matrix actualH, actualH2; + factor.evaluateError(T, baroBias, actualH, actualH2); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); +} + +// ************************************************************************* + +//*************************************************************************** +TEST(BarometricFactor, nonZero) { + using namespace example; + + // meters to barometric. + + double baroMeasurement = metersToBaro(10.); + + // Factor + Key key(1); + Key key2(2); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25); + BarometricFactor factor(key, key2, baroMeasurement, model); + + Pose3 T(Rot3::RzRyRx(0.5, 1., 1.), Point3(20., 30., 1.)); + double baroBias = 5.; + + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative21( + std::bind(&BarometricFactor::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, boost::none, + boost::none), + T, baroBias); + + Matrix expectedH2 = numericalDerivative22( + std::bind(&BarometricFactor::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, boost::none, + boost::none), + T, baroBias); + + // Use the factor to calculate the derivative and the error + Matrix actualH, actualH2; + Vector error = factor.evaluateError(T, baroBias, actualH, actualH2); + Vector actual = (Vector(1) << -4.0).finished(); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); + EXPECT(assert_equal(error, actual, 1e-8)); +} + +// ************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +// *************************************************************************