12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Apply Google Format

release/4.3a0
peterQFR 2021-12-20 07:24:52 +10:00
parent a7f6856d6a
commit cf0830084d
3 changed files with 149 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
gtsam/navigation/BarometricFactor.cpp
View File

@ -23,30 +23,31 @@ using namespace std;
namespace gtsam { 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"; cout << " Baro measurement: " << nT_ << "\n";
noiseModel_->print(" noise model: "); noiseModel_->print(" noise model: ");
} }
//*************************************************************************** //***************************************************************************
bool BarometricFactor::equals(const NonlinearFactor& expected, double tol) const { bool BarometricFactor::equals(const NonlinearFactor& expected,
const This* e = dynamic_cast<const This*>(&expected); double tol) const {
return e != nullptr && Base::equals(*e, tol) && traits<double>::Equals(nT_, e->nT_, tol); 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, 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 { boost::optional<Matrix&> H2) const {
if (H2) (*H2) = (Matrix(1, 1) << 1.0).finished();
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();
return (Vector(1) <<(p.translation().z()+bias - nT_)).finished();
} }
//*************************************************************************** } // namespace gtsam
}/// namespace gtsam

126
gtsam/navigation/BarometricFactor.h
View File

@ -17,9 +17,9 @@
**/ **/
#pragma once #pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam { namespace gtsam {
@ -31,83 +31,79 @@ namespace gtsam {
* https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html * https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html
* @addtogroup Navigation * @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 ~BarometricFactor() override {}
typedef boost::shared_ptr<BarometricFactor> shared_ptr;
/// 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 */ /// @return a deep copy of this factor
BarometricFactor(): nT_(0) {} 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;
/** /// equals
* @brief Constructor from a measurement of pressure in KPa. bool equals(const NonlinearFactor& expected,
* @param key of the Pose3 variable that will be constrained double tol = 1e-9) const override;
* @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 /// vector of errors
gtsam::NonlinearFactor::shared_ptr clone() const override { Vector evaluateError(
return boost::static_pointer_cast<gtsam::NonlinearFactor>( const Pose3& p, const double& b,
gtsam::NonlinearFactor::shared_ptr(new This(*this))); boost::optional<Matrix&> H = boost::none,
} boost::optional<Matrix&> H2 = boost::none) const override;
/// print inline const double& measurementIn() const { return nT_; }
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/// equals inline double heightOut(double n) const {
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; // 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 inline double baroOut(const double& meters) {
Vector evaluateError(const Pose3& p, const double& b, double temp = 15.04 - 0.00649 * meters;
boost::optional<Matrix&> H = boost::none, return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256);
boost::optional<Matrix&> H2 = boost::none) const override; };
inline const double & measurementIn() const { private:
return nT_; /// Serialization function
} friend class boost::serialization::access;
template <class ARCHIVE>
inline double heightOut(double n) const { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
//From https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html ar& boost::serialization::make_nvp(
return (std::pow(n/101.29, 1./5.256)*288.08 - 273.1 - 15.04)/-0.00649; "NoiseModelFactor1",
};
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)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(nT_); ar& BOOST_SERIALIZATION_NVP(nT_);
} }
}; };
} /// namespace gtsam } // namespace gtsam

141
gtsam/navigation/tests/testBarometricFactor.cpp
View File

@ -16,117 +16,114 @@
* @date 16 Dec, 2021 * @date 16 Dec, 2021
*/ */
#include <gtsam/navigation/BarometricFactor.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/navigation/BarometricFactor.h>
#include <boost/bind/bind.hpp> #include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace std::placeholders; using namespace std::placeholders;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// ************************************************************************* // *************************************************************************
namespace example { 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);
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 ) { TEST(BarometricFactor, Constructor) {
using namespace example; using namespace example;
//meters to barometric. // meters to barometric.
double baroMeasurement = metersToBaro(10.); double baroMeasurement = metersToBaro(10.);
// Factor // Factor
Key key(1); Key key(1);
Key key2(2); Key key2(2);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25); SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25);
BarometricFactor factor(key, key2, baroMeasurement, model); BarometricFactor factor(key, key2, baroMeasurement, model);
// Create a linearization point at zero error // Create a linearization point at zero error
Pose3 T(Rot3::RzRyRx(0., 0., 0.), Point3(0., 0., 10.)); Pose3 T(Rot3::RzRyRx(0., 0., 0.), Point3(0., 0., 10.));
double baroBias=0.; double baroBias = 0.;
Vector1 zero; Vector1 zero;
zero<< 0.; zero << 0.;
EXPECT(assert_equal(zero, factor.evaluateError(T, baroBias),1e-5)); EXPECT(assert_equal(zero, factor.evaluateError(T, baroBias), 1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative21<Vector,Pose3, double>( 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>( 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;
factor.evaluateError(T, baroBias, actualH, actualH2);
// Use the factor to calculate the derivative // Verify we get the expected error
Matrix actualH, actualH2; EXPECT(assert_equal(expectedH, actualH, 1e-8));
factor.evaluateError(T, baroBias, actualH, actualH2); 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) { TEST(BarometricFactor, nonZero) {
using namespace example; using namespace example;
//meters to barometric. // meters to barometric.
double baroMeasurement = metersToBaro(10.); double baroMeasurement = metersToBaro(10.);
// Factor // Factor
Key key(1); Key key(1);
Key key2(2); Key key2(2);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25); SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25);
BarometricFactor factor(key, key2, baroMeasurement, model); BarometricFactor factor(key, key2, baroMeasurement, model);
Pose3 T(Rot3::RzRyRx(0.5, 1., 1.), Point3(20., 30., 1.)); Pose3 T(Rot3::RzRyRx(0.5, 1., 1.), Point3(20., 30., 1.));
double baroBias=5.; 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));
// 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() { int main() {
TestResult tr; TestResult tr;
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
// ************************************************************************* // *************************************************************************