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) const {
        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