Apply Google Format
peterQFR
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