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

reverting changes to existing factors

release/4.3a0
morten 2025-01-15 11:54:41 +01:00
parent 7efcb468f5
commit af1e6e34e6
4 changed files with 18 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/navigation/GPSFactor.cpp
View File

@ -26,31 +26,20 @@ namespace gtsam {
void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << (s.empty() ? "" : s + " ") << "GPSFactor on " << keyFormatter(key()) cout << (s.empty() ? "" : s + " ") << "GPSFactor on " << keyFormatter(key())
<< "\n"; << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n"; cout << " GPS measurement: " << nT_ << "\n";
cout << " Lever arm: " << B_t_BG_.transpose() << "\n";
noiseModel_->print(" noise model: "); noiseModel_->print(" noise model: ");
} }
//*************************************************************************** //***************************************************************************
bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) && return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(nT_, e->nT_, tol);
traits<Point3>::Equals(nT_, e->nT_, tol) &&
traits<Point3>::Equals(B_t_BG_, e->B_t_BG_, tol);
} }
//*************************************************************************** //***************************************************************************
Vector GPSFactor::evaluateError(const Pose3& p, Vector GPSFactor::evaluateError(const Pose3& p,
OptionalMatrixType H) const { OptionalMatrixType H) const {
const Matrix3 rot = p.rotation().matrix(); return p.translation(H) -nT_;
if (H) {
H->resize(3, 6);
H->block<3, 3>(0, 0) = -rot * skewSymmetric(B_t_BG_);
H->block<3, 3>(0, 3) = rot;
}
return p.translation() - (nT_ - rot * B_t_BG_);
} }
//*************************************************************************** //***************************************************************************
@ -78,8 +67,7 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
//*************************************************************************** //***************************************************************************
void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n"; cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n"; cout << " GPS measurement: " << nT_.transpose() << endl;
cout << " Lever arm: " << B_t_BG_.transpose() << "\n";
noiseModel_->print(" noise model: "); noiseModel_->print(" noise model: ");
} }
@ -87,23 +75,13 @@ void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const
bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) && return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol) && traits<Point3>::Equals(nT_, e->nT_, tol);
traits<Point3>::Equals(B_t_BG_, e->B_t_BG_, tol);
} }
//*************************************************************************** //***************************************************************************
Vector GPSFactor2::evaluateError(const NavState& p, Vector GPSFactor2::evaluateError(const NavState& p,
OptionalMatrixType H) const { OptionalMatrixType H) const {
const Matrix3 rot = p.attitude().matrix(); return p.position(H) -nT_;
if (H) {
H->resize(3, 9);
H->block<3, 3>(0, 0) = -rot * skewSymmetric(B_t_BG_);
H->block<3, 3>(0, 3) = rot;
H->block<3, 3>(0, 6).setZero();
}
return p.position() - (nT_ - rot * B_t_BG_);
} }
//*************************************************************************** //***************************************************************************

24
gtsam/navigation/GPSFactor.h
View File

@ -39,7 +39,6 @@ private:
typedef NoiseModelFactorN<Pose3> Base; typedef NoiseModelFactorN<Pose3> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates Point3 nT_; ///< Position measurement in cartesian coordinates
Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame
public: public:
@ -53,7 +52,7 @@ public:
typedef GPSFactor This; typedef GPSFactor This;
/** default constructor - only use for serialization */ /** default constructor - only use for serialization */
GPSFactor(): nT_(0, 0, 0), B_t_BG_(0, 0, 0) {} GPSFactor(): nT_(0, 0, 0) {}
~GPSFactor() override {} ~GPSFactor() override {}
@ -64,8 +63,8 @@ public:
* @param gpsIn measurement already in correct coordinates * @param gpsIn measurement already in correct coordinates
* @param model Gaussian noise model * @param model Gaussian noise model
*/ */
GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) : GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) { Base(model, key), nT_(gpsIn) {
} }
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -88,10 +87,6 @@ public:
return nT_; return nT_;
} }
inline const Point3 & leverArm() const {
return B_t_BG_;
}
/** /**
* Convenience function to estimate state at time t, given two GPS * Convenience function to estimate state at time t, given two GPS
* readings (in local NED Cartesian frame) bracketing t * readings (in local NED Cartesian frame) bracketing t
@ -112,7 +107,6 @@ private:
& boost::serialization::make_nvp("NoiseModelFactor1", & 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_);
ar & BOOST_SERIALIZATION_NVP(B_t_BG_);
} }
#endif #endif
}; };
@ -128,7 +122,6 @@ private:
typedef NoiseModelFactorN<NavState> Base; typedef NoiseModelFactorN<NavState> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates Point3 nT_; ///< Position measurement in cartesian coordinates
Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame
public: public:
@ -142,13 +135,13 @@ public:
typedef GPSFactor2 This; typedef GPSFactor2 This;
/// default constructor - only use for serialization /// default constructor - only use for serialization
GPSFactor2():nT_(0, 0, 0), B_t_BG_(0, 0, 0) {} GPSFactor2():nT_(0, 0, 0) {}
~GPSFactor2() override {} ~GPSFactor2() override {}
/// Constructor from a measurement in a Cartesian frame. /// Constructor from a measurement in a Cartesian frame.
GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) : GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) { Base(model, key), nT_(gpsIn) {
} }
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -171,10 +164,6 @@ public:
return nT_; return nT_;
} }
inline const Point3 & leverArm() const {
return B_t_BG_;
}
private: private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION #if GTSAM_ENABLE_BOOST_SERIALIZATION
@ -187,7 +176,6 @@ private:
& boost::serialization::make_nvp("NoiseModelFactor1", & 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_);
ar & BOOST_SERIALIZATION_NVP(B_t_BG_);
} }
#endif #endif
}; };

8
gtsam/navigation/navigation.i
View File

@ -329,8 +329,7 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor {
#include <gtsam/navigation/GPSFactor.h> #include <gtsam/navigation/GPSFactor.h>
virtual class GPSFactor : gtsam::NonlinearFactor{ virtual class GPSFactor : gtsam::NonlinearFactor{
GPSFactor(size_t key, const gtsam::Point3& gpsIn, GPSFactor(size_t key, const gtsam::Point3& gpsIn,
const gtsam::noiseModel::Base* model, const gtsam::noiseModel::Base* model);
const gtsam::Point3& leverArm);
// Testable // Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter = void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
@ -339,7 +338,6 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
// Standard Interface // Standard Interface
gtsam::Point3 measurementIn() const; gtsam::Point3 measurementIn() const;
gtsam::Point3 leverArm() const;
// enable serialization functionality // enable serialization functionality
void serialize() const; void serialize() const;
@ -347,8 +345,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
virtual class GPSFactor2 : gtsam::NonlinearFactor { virtual class GPSFactor2 : gtsam::NonlinearFactor {
GPSFactor2(size_t key, const gtsam::Point3& gpsIn, GPSFactor2(size_t key, const gtsam::Point3& gpsIn,
const gtsam::noiseModel::Base* model, const gtsam::noiseModel::Base* model);
const gtsam::Point3& leverArm);
// Testable // Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter = void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
@ -357,7 +354,6 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {
// Standard Interface // Standard Interface
gtsam::Point3 measurementIn() const; gtsam::Point3 measurementIn() const;
gtsam::Point3 leverArm() const;
// enable serialization functionality // enable serialization functionality
void serialize() const; void serialize() const;

15
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -45,9 +45,6 @@ LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);
// Dekalb-Peachtree Airport runway 2L // Dekalb-Peachtree Airport runway 2L
const double lat = 33.87071, lon = -84.30482, h = 274; const double lat = 33.87071, lon = -84.30482, h = 274;
// Random lever arm
const Point3 leverArm(0.1, 0.2, 0.3);
} }
// ************************************************************************* // *************************************************************************
@ -64,12 +61,10 @@ TEST( GPSFactor, Constructor ) {
// Factor // Factor
Key key(1); Key key(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactor factor(key, Point3(E, N, U), model, leverArm); GPSFactor factor(key, Point3(E, N, U), model);
// Create a linearization point at zero error // Create a linearization point at zero error
const Rot3 rot = Rot3::RzRyRx(0.15, -0.30, 0.45); Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U));
const Point3 p = Point3(E, N, U) - rot * leverArm;
Pose3 T(rot, p);
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives
@ -95,12 +90,10 @@ TEST( GPSFactor2, Constructor ) {
// Factor // Factor
Key key(1); Key key(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactor2 factor(key, Point3(E, N, U), model, leverArm); GPSFactor2 factor(key, Point3(E, N, U), model);
// Create a linearization point at zero error // Create a linearization point at zero error
const Rot3 rot = Rot3::RzRyRx(0.15, -0.30, 0.45); NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero());
const Point3 p = Point3(E, N, U) - rot * leverArm;
NavState T(rot, p, Vector3::Zero());
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives