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:

    // Provide access to the Matrix& version of evaluateError:
    using Base::evaluateError;

    typedef std::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 std::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, 
            OptionalMatrixType H, OptionalMatrixType H2) 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:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    
    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_);
    }
#endif
};

}  // namespace gtsam