Apply Google Format
							parent
							
								
									a7f6856d6a
								
							
						
					
					
						commit
						cf0830084d
					
				|  | @ -23,30 +23,31 @@ 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"; | ||||
| 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: "); | ||||
|     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); | ||||
| 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(); | ||||
| 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
 | ||||
| }  // namespace gtsam
 | ||||
|  |  | |||
|  | @ -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 { | ||||
| 
 | ||||
|  | @ -31,83 +31,79 @@ namespace gtsam { | |||
|  * https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html
 | ||||
|  * @addtogroup Navigation | ||||
|  */ | ||||
| class GTSAM_EXPORT BarometricFactor: public NoiseModelFactor2<Pose3, double> { | ||||
| class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2<Pose3, double> { | ||||
|    private: | ||||
|     typedef NoiseModelFactor2<Pose3, double> Base; | ||||
| 
 | ||||
| private: | ||||
|     double nT_;  ///< Height Measurement based on a standard atmosphere
 | ||||
| 
 | ||||
|   typedef NoiseModelFactor2<Pose3, double> Base; | ||||
|    public: | ||||
|     /// shorthand for a smart pointer to a factor
 | ||||
|     typedef boost::shared_ptr<BarometricFactor> shared_ptr; | ||||
| 
 | ||||
|   double nT_; ///< Height Measurement based on a standard atmosphere
 | ||||
|     /// Typedef to this class
 | ||||
|     typedef BarometricFactor This; | ||||
| 
 | ||||
| public: | ||||
|     /** default constructor - only use for serialization */ | ||||
|     BarometricFactor() : nT_(0) {} | ||||
| 
 | ||||
|   /// shorthand for a smart pointer to a factor
 | ||||
|   typedef boost::shared_ptr<BarometricFactor> shared_ptr; | ||||
|     ~BarometricFactor() override {} | ||||
| 
 | ||||
|   /// Typedef to this class
 | ||||
|   typedef BarometricFactor This; | ||||
|     /**
 | ||||
|      * @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)) {} | ||||
| 
 | ||||
|   /** default constructor - only use for serialization */ | ||||
|   BarometricFactor(): nT_(0) {} | ||||
|     /// @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))); | ||||
|     } | ||||
| 
 | ||||
|   ~BarometricFactor() override {} | ||||
|     /// print
 | ||||
|     void print( | ||||
|         const std::string& s = "", | ||||
|         const KeyFormatter& keyFormatter = DefaultKeyFormatter) const 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)) { | ||||
|   } | ||||
|     /// equals
 | ||||
|     bool equals(const NonlinearFactor& expected, | ||||
|                 double tol = 1e-9) const override; | ||||
| 
 | ||||
|   /// @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))); | ||||
|   } | ||||
|     /// 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; | ||||
| 
 | ||||
|   /// print
 | ||||
|   void print(const std::string& s = "", const KeyFormatter& keyFormatter = | ||||
|                                             DefaultKeyFormatter) const override; | ||||
|     inline const double& measurementIn() const { return nT_; } | ||||
| 
 | ||||
|   /// equals
 | ||||
|   bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; | ||||
|     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; | ||||
|     }; | ||||
| 
 | ||||
|   /// 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 double baroOut(const double& meters) { | ||||
|         double temp = 15.04 - 0.00649 * meters; | ||||
|         return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256); | ||||
|     }; | ||||
| 
 | ||||
|   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", | ||||
|    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_); | ||||
|   } | ||||
|         ar& BOOST_SERIALIZATION_NVP(nT_); | ||||
|     } | ||||
| }; | ||||
| 
 | ||||
| } /// namespace gtsam
 | ||||
| }  // namespace gtsam
 | ||||
|  |  | |||
|  | @ -16,117 +16,114 @@ | |||
|  * @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 { | ||||
| } | ||||
| 
 | ||||
| double metersToBaro(const double& meters) | ||||
| { | ||||
|     double temp = 15.04 - 0.00649*meters; | ||||
|     return 101.29*std::pow(((temp+273.1)/288.08), 5.256); | ||||
| 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; | ||||
| TEST(BarometricFactor, Constructor) { | ||||
|     using namespace example; | ||||
| 
 | ||||
|   //meters to barometric.
 | ||||
|     // meters to barometric.
 | ||||
| 
 | ||||
|   double baroMeasurement = metersToBaro(10.); | ||||
|     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); | ||||
|     // 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)); | ||||
|     // 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); | ||||
|     // 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); | ||||
|     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); | ||||
| 
 | ||||
|   // 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)); | ||||
|     // 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; | ||||
|     using namespace example; | ||||
| 
 | ||||
|   //meters to barometric.
 | ||||
|     // meters to barometric.
 | ||||
| 
 | ||||
|   double baroMeasurement = metersToBaro(10.); | ||||
|     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); | ||||
|     // 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)); | ||||
|     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); | ||||
|     TestResult tr; | ||||
|     return TestRegistry::runAllTests(tr); | ||||
| } | ||||
| // *************************************************************************
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue