Add 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,52 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 {
+
+  if (H2) (*H2) = (Matrix(1,1) << 1.0).finished();
+  if(H)(*H) = (Matrix(1, 6)<< 0., 0., 0.,0., 0., 1.).finished();
+  return (Vector(1) <<(p.translation().z()+bias - nT_)).finished();
+}
+
+//***************************************************************************
+
+}/// namespace gtsam

gtsam/navigation/BarometricFactor.h

@@ -0,0 +1,113 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/nonlinear/NonlinearFactor.h>
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/geometry/Pose3.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,118 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 <gtsam/navigation/BarometricFactor.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+
+#include <boost/bind/bind.hpp>
+
+#include <CppUnitLite/TestHarness.h>
+
+
+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(GPSData, init) {
+
+  /* GPS Reading 1 will be ENU origin
+  double t1 = 84831;
+  Point3 NED1(0, 0, 0);
+  LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54, kWGS84);
+
+  // GPS Readin 2
+  double t2 = 84831.5;
+  double E, N, U;
+  enu.Forward(35.4394633333333, -119.063146666667, 276.52, E, N, U);
+  Point3 NED2(N, E, -U);
+
+  // Estimate initial state
+  Pose3 T;
+  Vector3 nV;
+  boost::tie(T, nV) = GPSFactor::EstimateState(t1, NED1, t2, NED2, 84831.0796);
+
+  // Check values values
+  EXPECT(assert_equal((Vector )Vector3(29.9575, -29.0564, -1.95993), nV, 1e-4));
+  EXPECT( assert_equal(Rot3::Ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5));
+  Point3 expectedT(2.38461, -2.31289, -0.156011);
+  EXPECT(assert_equal(expectedT, T.translation(), 1e-5));
+  */
+}
+
+// *************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+// *************************************************************************