Apply Google Format

gtsam/navigation/BarometricFactor.cpp

@@ -23,30 +23,31 @@ using namespace std;
 namespace gtsam {
 
 //***************************************************************************
-void BarometricFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
+void BarometricFactor::print(const string& s,
-  cout << (s.empty() ? "" : s + " ") << "Barometric Factor on " << keyFormatter(key1())
+                             const KeyFormatter& keyFormatter) const {
-       << "Barometric Bias on " << keyFormatter(key2()) << "\n";
+    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 {
+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);
+    return e != nullptr && Base::equals(*e, tol) &&
+           traits<double>::Equals(nT_, e->nT_, tol);
 }
 
 //***************************************************************************
-Vector BarometricFactor::evaluateError(const Pose3& p,
+Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias,
-        const double& bias, boost::optional<Matrix&> H,
+                                       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
-
-}/// namespace gtsam

gtsam/navigation/BarometricFactor.h

@@ -17,9 +17,9 @@
  **/
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/navigation/NavState.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
 
@@ -32,15 +32,12 @@ namespace gtsam {
  * @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;
 
@@ -59,9 +56,9 @@ public:
      * @param baroIn measurement in KPa
      * @param model Gaussian noise model 1 dimension
      */
-  BarometricFactor(Key key, Key baroKey, const double& baroIn, const SharedNoiseModel& model) :
+    BarometricFactor(Key key, Key baroKey, const double& baroIn,
-      Base(model, key, baroKey), nT_(heightOut(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 {
@@ -70,44 +67,43 @@ public:
     }
 
     /// print
-  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
+    void print(
-                                            DefaultKeyFormatter) const override;
+        const std::string& s = "",
+        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
     /// equals
-  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
+    bool equals(const NonlinearFactor& expected,
+                double tol = 1e-9) const override;
 
     /// vector of errors
-  Vector evaluateError(const Pose3& p, const double& b,
+    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 {
+    inline const double& measurementIn() const { return nT_; }
-    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;
+        return (std::pow(n / 101.29, 1. / 5.256) * 288.08 - 273.1 - 15.04) /
-
+               -0.00649;
     };
 
-  inline double   baroOut(const double& meters)
+    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
+        ar& boost::serialization::make_nvp(
-        & boost::serialization::make_nvp("NoiseModelFactor1",
+            "NoiseModelFactor1",
             boost::serialization::base_object<Base>(*this));
         ar& BOOST_SERIALIZATION_NVP(nT_);
     }
 };
 
-} /// namespace gtsam
+}  // namespace gtsam

gtsam/navigation/tests/testBarometricFactor.cpp

@@ -16,29 +16,23 @@
  * @date   16 Dec, 2021
  */
 
-#include <gtsam/navigation/BarometricFactor.h>
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/navigation/BarometricFactor.h>
 
 #include <boost/bind/bind.hpp>
 
-#include <CppUnitLite/TestHarness.h>
-
-
 using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
-
 // *************************************************************************
-namespace example {
+namespace example {}
-}
 
-double metersToBaro(const double& meters)
+double metersToBaro(const double& meters) {
-{
     double temp = 15.04 - 0.00649 * meters;
     return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256);
-
 }
 
 // *************************************************************************
@@ -64,13 +58,16 @@ TEST( BarometricFactor, Constructor ) {
 
     // Calculate numerical derivatives
     Matrix expectedH = numericalDerivative21<Vector, Pose3, double>(
-      std::bind(&BarometricFactor::evaluateError, &factor, std::placeholders::_1,
+        std::bind(&BarometricFactor::evaluateError, &factor,
-          std::placeholders::_2, boost::none, boost::none), T, baroBias);
+                  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::bind(&BarometricFactor::evaluateError, &factor,
-          std::placeholders::_2, boost::none, boost::none), T, baroBias);
+                  std::placeholders::_1, std::placeholders::_2, boost::none,
-
+                  boost::none),
+        T, baroBias);
 
     // Use the factor to calculate the derivative
     Matrix actualH, actualH2;
@@ -102,12 +99,16 @@ TEST(BarometricFactor, nonZero) {
 
     // Calculate numerical derivatives
     Matrix expectedH = numericalDerivative21<Vector, Pose3, double>(
-      std::bind(&BarometricFactor::evaluateError, &factor, std::placeholders::_1,
+        std::bind(&BarometricFactor::evaluateError, &factor,
-          std::placeholders::_2, boost::none, boost::none), T, baroBias);
+                  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::bind(&BarometricFactor::evaluateError, &factor,
-          std::placeholders::_2, boost::none, boost::none), T, baroBias);
+                  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;
@@ -118,10 +119,6 @@ TEST(BarometricFactor, nonZero) {
     EXPECT(assert_equal(expectedH, actualH, 1e-8));
     EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
     EXPECT(assert_equal(error, actual, 1e-8));
-
-
-
-
 }
 
 // *************************************************************************