Merge pull request #969 from PeterQFR/barometric_factor

gtsam/CMakeLists.txt

@@ -15,7 +15,7 @@ set (gtsam_subdirs
     sam
     sfm
     slam
-	  navigation
+    navigation
 )
 
 set(gtsam_srcs)

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<const This*>(&expected);
+    return e != nullptr && Base::equals(*e, tol) &&
+           traits<double>::Equals(nT_, e->nT_, tol);
+}
+
+//***************************************************************************
+Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias,
+                                       boost::optional<Matrix&> H,
+                                       boost::optional<Matrix&> 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

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 <gtsam/geometry/Pose3.h>
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+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<Pose3, double> {
+   private:
+    typedef NoiseModelFactor2<Pose3, double> Base;
+
+    double nT_;  ///< Height Measurement based on a standard atmosphere
+
+   public:
+    /// shorthand for a smart pointer to a factor
+    typedef boost::shared_ptr<BarometricFactor> 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>(
+            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<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:
+    /// Serialization function
+    friend class boost::serialization::access;
+    template <class ARCHIVE>
+    void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+        ar& boost::serialization::make_nvp(
+            "NoiseModelFactor1",
+            boost::serialization::base_object<Base>(*this));
+        ar& BOOST_SERIALIZATION_NVP(nT_);
+    }
+};
+
+}  // namespace gtsam

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 <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/navigation/BarometricFactor.h>
+
+#include <boost/bind/bind.hpp>
+
+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<Vector, Pose3, double>(
+        std::bind(&BarometricFactor::evaluateError, &factor,
+                  std::placeholders::_1, std::placeholders::_2, boost::none,
+                  boost::none),
+        T, baroBias);
+
+    Matrix expectedH2 = numericalDerivative22<Vector, Pose3, double>(
+        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<Vector, Pose3, double>(
+        std::bind(&BarometricFactor::evaluateError, &factor,
+                  std::placeholders::_1, std::placeholders::_2, boost::none,
+                  boost::none),
+        T, baroBias);
+
+    Matrix expectedH2 = numericalDerivative22<Vector, Pose3, double>(
+        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);
+}
+// *************************************************************************