From e6d29a4545b0cda6b122516f6fcdd044dc17a648 Mon Sep 17 00:00:00 2001 From: peterQFR Date: Wed, 15 Dec 2021 16:39:11 +1000 Subject: [PATCH 1/7] Set the install path to be colconone --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5fd5d521c..a32f3b076 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ set (CMAKE_PROJECT_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) set (CMAKE_PROJECT_VERSION_MINOR ${GTSAM_VERSION_MINOR}) set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH}) +set (SMAKE_INSTALL_PREFIX:PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../install/) ############################################################################### # Gather information, perform checks, set defaults From 6813e2a3fcf066209c526aa0865409ad0164640a Mon Sep 17 00:00:00 2001 From: peterQFR Date: Fri, 17 Dec 2021 11:58:21 +1000 Subject: [PATCH 2/7] Add Barometric Factor --- gtsam/CMakeLists.txt | 2 +- gtsam/navigation/BarometricFactor.cpp | 52 ++++++++ gtsam/navigation/BarometricFactor.h | 113 +++++++++++++++++ .../navigation/tests/testBarometricFactor.cpp | 118 ++++++++++++++++++ 4 files changed, 284 insertions(+), 1 deletion(-) create mode 100644 gtsam/navigation/BarometricFactor.cpp create mode 100644 gtsam/navigation/BarometricFactor.h create mode 100644 gtsam/navigation/tests/testBarometricFactor.cpp diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 535d60eb1..a293c6ec2 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -15,7 +15,7 @@ set (gtsam_subdirs sam sfm slam - navigation + navigation ) set(gtsam_srcs) diff --git a/gtsam/navigation/BarometricFactor.cpp b/gtsam/navigation/BarometricFactor.cpp new file mode 100644 index 000000000..98c280b69 --- /dev/null +++ b/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(&expected); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); +} + +//*************************************************************************** +Vector BarometricFactor::evaluateError(const Pose3& p, + const double& bias, boost::optional H, + boost::optional 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 diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h new file mode 100644 index 000000000..c7330031a --- /dev/null +++ b/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 +#include +#include + +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 { + +private: + + typedef NoiseModelFactor2 Base; + + double nT_; ///< Height Measurement based on a standard atmosphere + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr 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::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 H = boost::none, + boost::optional 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 + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(nT_); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/tests/testBarometricFactor.cpp b/gtsam/navigation/tests/testBarometricFactor.cpp new file mode 100644 index 000000000..a3ac7b0c0 --- /dev/null +++ b/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 +#include +#include + +#include + +#include + + +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( + std::bind(&BarometricFactor::evaluateError, &factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), T, baroBias); + + Matrix expectedH2 = numericalDerivative22( + 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); +} +// ************************************************************************* From ae47ffee29f9b6c900c07554d5648e97d3c2d4c4 Mon Sep 17 00:00:00 2001 From: peterQFR Date: Fri, 17 Dec 2021 12:18:13 +1000 Subject: [PATCH 3/7] Remove custom install for ament environment --- CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a32f3b076..5fd5d521c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,7 +19,6 @@ set (CMAKE_PROJECT_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) set (CMAKE_PROJECT_VERSION_MINOR ${GTSAM_VERSION_MINOR}) set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH}) -set (SMAKE_INSTALL_PREFIX:PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../install/) ############################################################################### # Gather information, perform checks, set defaults From a7f6856d6ab2b0b8e66f139961e4b694505bb3a4 Mon Sep 17 00:00:00 2001 From: peterQFR Date: Mon, 20 Dec 2021 06:50:29 +1000 Subject: [PATCH 4/7] Add non-zero tests, error --- gtsam/navigation/BarometricFactor.cpp | 2 +- .../navigation/tests/testBarometricFactor.cpp | 54 ++++++++++++------- 2 files changed, 35 insertions(+), 21 deletions(-) diff --git a/gtsam/navigation/BarometricFactor.cpp b/gtsam/navigation/BarometricFactor.cpp index 98c280b69..82293d49c 100644 --- a/gtsam/navigation/BarometricFactor.cpp +++ b/gtsam/navigation/BarometricFactor.cpp @@ -43,7 +43,7 @@ Vector BarometricFactor::evaluateError(const Pose3& p, boost::optional H2) const { if (H2) (*H2) = (Matrix(1,1) << 1.0).finished(); - if(H)(*H) = (Matrix(1, 6)<< 0., 0., 0.,0., 0., 1.).finished(); + if(H)(*H) = (Matrix(1, 6) << 0., 0., 0., 0., 0., 1.).finished(); return (Vector(1) <<(p.translation().z()+bias - nT_)).finished(); } diff --git a/gtsam/navigation/tests/testBarometricFactor.cpp b/gtsam/navigation/tests/testBarometricFactor.cpp index a3ac7b0c0..3ef0a7c10 100644 --- a/gtsam/navigation/tests/testBarometricFactor.cpp +++ b/gtsam/navigation/tests/testBarometricFactor.cpp @@ -84,30 +84,44 @@ TEST( BarometricFactor, Constructor ) { // ************************************************************************* //*************************************************************************** -TEST(GPSData, init) { +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( + std::bind(&BarometricFactor::evaluateError, &factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), T, baroBias); + + Matrix expectedH2 = numericalDerivative22( + 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)); - /* 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)); - */ } // ************************************************************************* From cf0830084d3b7495cfecbddb69701a960ccd168d Mon Sep 17 00:00:00 2001 From: peterQFR Date: Mon, 20 Dec 2021 07:24:52 +1000 Subject: [PATCH 5/7] Apply Google Format --- gtsam/navigation/BarometricFactor.cpp | 37 ++--- gtsam/navigation/BarometricFactor.h | 126 ++++++++-------- .../navigation/tests/testBarometricFactor.cpp | 141 +++++++++--------- 3 files changed, 149 insertions(+), 155 deletions(-) diff --git a/gtsam/navigation/BarometricFactor.cpp b/gtsam/navigation/BarometricFactor.cpp index 82293d49c..0fcdc6180 100644 --- a/gtsam/navigation/BarometricFactor.cpp +++ b/gtsam/navigation/BarometricFactor.cpp @@ -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(&expected); - return e != nullptr && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); +bool BarometricFactor::equals(const NonlinearFactor& expected, + double tol) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** -Vector BarometricFactor::evaluateError(const Pose3& p, - const double& bias, boost::optional H, - boost::optional 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 H, + boost::optional 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 diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h index c7330031a..e7bf6f998 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -17,9 +17,9 @@ **/ #pragma once -#include -#include #include +#include +#include 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 { +class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2 { + private: + typedef NoiseModelFactor2 Base; -private: + double nT_; ///< Height Measurement based on a standard atmosphere - typedef NoiseModelFactor2 Base; + public: + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr 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 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::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::shared_ptr(new This(*this))); - } + /// vector of errors + Vector evaluateError( + const Pose3& p, const double& b, + boost::optional H = boost::none, + boost::optional 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 H = boost::none, - boost::optional 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 - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar - & boost::serialization::make_nvp("NoiseModelFactor1", + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor1", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(nT_); - } + ar& BOOST_SERIALIZATION_NVP(nT_); + } }; -} /// namespace gtsam +} // namespace gtsam diff --git a/gtsam/navigation/tests/testBarometricFactor.cpp b/gtsam/navigation/tests/testBarometricFactor.cpp index 3ef0a7c10..47f4824c1 100644 --- a/gtsam/navigation/tests/testBarometricFactor.cpp +++ b/gtsam/navigation/tests/testBarometricFactor.cpp @@ -16,117 +16,114 @@ * @date 16 Dec, 2021 */ -#include +#include #include #include +#include #include -#include - - 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( - std::bind(&BarometricFactor::evaluateError, &factor, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none), T, baroBias); + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative21( + std::bind(&BarometricFactor::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, boost::none, + boost::none), + T, baroBias); - Matrix expectedH2 = numericalDerivative22( - std::bind(&BarometricFactor::evaluateError, &factor, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none), T, baroBias); + Matrix expectedH2 = numericalDerivative22( + 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( - std::bind(&BarometricFactor::evaluateError, &factor, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none), T, baroBias); - - Matrix expectedH2 = numericalDerivative22( - 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( + std::bind(&BarometricFactor::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, boost::none, + boost::none), + T, baroBias); + Matrix expectedH2 = numericalDerivative22( + 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); } // ************************************************************************* From 66c8ca4af033bf1625f0c96d1bdac5255cf4ffe7 Mon Sep 17 00:00:00 2001 From: peterQFR Date: Tue, 21 Dec 2021 08:33:09 +1000 Subject: [PATCH 6/7] Use translation method to get jacobian for pose in pose coordinates --- gtsam/navigation/BarometricFactor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/BarometricFactor.cpp b/gtsam/navigation/BarometricFactor.cpp index 0fcdc6180..3246bed68 100644 --- a/gtsam/navigation/BarometricFactor.cpp +++ b/gtsam/navigation/BarometricFactor.cpp @@ -45,9 +45,11 @@ bool BarometricFactor::equals(const NonlinearFactor& expected, Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias, boost::optional H, boost::optional 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(); - if (H) (*H) = (Matrix(1, 6) << 0., 0., 0., 0., 0., 1.).finished(); - return (Vector(1) << (p.translation().z() + bias - nT_)).finished(); + return ret; } } // namespace gtsam From cc5c5c06ea4048450a788dcdd961abefe345d0c9 Mon Sep 17 00:00:00 2001 From: peterQFR Date: Tue, 21 Dec 2021 08:41:47 +1000 Subject: [PATCH 7/7] Apply google format --- gtsam/navigation/BarometricFactor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/BarometricFactor.cpp b/gtsam/navigation/BarometricFactor.cpp index 3246bed68..2f0ff7436 100644 --- a/gtsam/navigation/BarometricFactor.cpp +++ b/gtsam/navigation/BarometricFactor.cpp @@ -46,8 +46,8 @@ Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias, boost::optional H, boost::optional H2) const { Matrix tH; - Vector ret =(Vector(1) << (p.translation(tH).z() + bias - nT_)).finished(); - if (H) (*H) = tH.block<1,6>(2,0); + 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; }