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

adding GPSFactors with lever arm as state

release/4.3a0
morten 2025-01-16 21:27:42 +01:00
parent 0c13038ed5
commit d1106ae4a3
3 changed files with 276 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
gtsam/navigation/GPSFactor.cpp
View File

@ -95,6 +95,38 @@ Vector GPSFactorArm::evaluateError(const Pose3& p,
return p.translation() + nRb * bL_ - nT_; return p.translation() + nRb * bL_ - nT_;
} }
//***************************************************************************
void GPSFactorArmCalib::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactorArmCalib on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool GPSFactorArmCalib::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol);
}
//***************************************************************************
Vector GPSFactorArmCalib::evaluateError(const Pose3& p, const Point3& bL,
OptionalMatrixType H1, OptionalMatrixType H2) const {
const Matrix3 nRb = p.rotation().matrix();
if (H1) {
H1->resize(3, 6);
H1->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL);
H1->block<3, 3>(0, 3) = nRb;
}
if (H2){
H2->resize(3, 3);
*H2 = nRb;
}
return p.translation() + nRb * bL - nT_;
}
//*************************************************************************** //***************************************************************************
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";
@ -146,4 +178,37 @@ Vector GPSFactor2Arm::evaluateError(const NavState& p,
return p.position() + nRb * bL_ - nT_; return p.position() + nRb * bL_ - nT_;
} }
//***************************************************************************
void GPSFactor2ArmCalib::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor2ArmCalib on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool GPSFactor2ArmCalib::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol);
}
//***************************************************************************
Vector GPSFactor2ArmCalib::evaluateError(const NavState& p, const Point3& bL,
OptionalMatrixType H1, OptionalMatrixType H2) const {
const Matrix3 nRb = p.attitude().matrix();
if (H1) {
H1->resize(3, 9);
H1->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL);
H1->block<3, 3>(0, 3) = nRb;
H1->block<3, 3>(0, 6).setZero();
}
if (H2){
H2->resize(3, 3);
*H2 = nRb;
}
return p.position() + nRb * bL - nT_;
}
}/// namespace gtsam }/// namespace gtsam

131
gtsam/navigation/GPSFactor.h
View File

@ -185,6 +185,72 @@ public:
}; };
/**
* Version of GPSFactorArm (for Pose3) with unknown lever arm between GPS and
* Body frame. Because the actual location of the antenna depends on both
* position and attitude, providing a lever arm makes components of the attitude
* observable and accounts for position measurement vs state discrepancies while
* turning.
* @ingroup navigation
*/
class GTSAM_EXPORT GPSFactorArmCalib: public NoiseModelFactorN<Pose3, Point3> {
private:
typedef NoiseModelFactorN<Pose3, Point3> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef std::shared_ptr<GPSFactorArmCalib> shared_ptr;
/// Typedef to this class
typedef GPSFactorArmCalib This;
/// default constructor - only use for serialization
GPSFactorArmCalib() : nT_(0, 0, 0) {}
~GPSFactorArmCalib() override {}
/** Constructor from a measurement in a Cartesian frame.
* @param key1 key of the Pose3 variable related to this measurement
* @param key2 key of the Point3 variable related to the lever arm
* @param gpsIn gps measurement, in Cartesian navigation frame
* @param leverArm translation from the body frame origin to the gps antenna, in body frame
* @param model Gaussian noise model
*/
GPSFactorArmCalib(Key key1, Key key2, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key1, key2), nT_(gpsIn) {
}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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 Point3& bL, OptionalMatrixType H1,
OptionalMatrixType H2) const override;
/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
return nT_;
}
};
/** /**
* Version of GPSFactor for NavState, assuming zero lever arm between body frame * Version of GPSFactor for NavState, assuming zero lever arm between body frame
* and GPS. If there exists a non-zero lever arm between body frame and GPS * and GPS. If there exists a non-zero lever arm between body frame and GPS
@ -332,4 +398,69 @@ public:
}; };
/**
* Version of GPSFactor2Arm for an unknown lever arm between GPS and Body frame.
* Because the actual location of the antenna depends on both position and
* attitude, providing a lever arm makes components of the attitude observable
* and accounts for position measurement vs state discrepancies while turning.
* @ingroup navigation
*/
class GTSAM_EXPORT GPSFactor2ArmCalib: public NoiseModelFactorN<NavState, Point3> {
private:
typedef NoiseModelFactorN<NavState, Point3> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef std::shared_ptr<GPSFactor2ArmCalib> shared_ptr;
/// Typedef to this class
typedef GPSFactor2ArmCalib This;
/// default constructor - only use for serialization
GPSFactor2ArmCalib():nT_(0, 0, 0) {}
~GPSFactor2ArmCalib() override {}
/** Constructor from a measurement in a Cartesian frame.
* @param key1 key of the NavState variable related to this measurement
* @param key2 key of the Point3 variable related to the lever arm
* @param gpsIn gps measurement, in Cartesian navigation frame
* @param model Gaussian noise model
*/
GPSFactor2ArmCalib(Key key1, Key key2, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key1, key2), nT_(gpsIn) {
}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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 NavState& p, const Point3& bL,
OptionalMatrixType H1,
OptionalMatrixType H2) const override;
/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
return nT_;
}
};
} /// namespace gtsam } /// namespace gtsam

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

@ -114,6 +114,46 @@ TEST( GPSFactorArm, Constructor ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-8));
} }
// *************************************************************************
TEST( GPSFactorArmCalib, Constructor ) {
using namespace example;
// From lat-lon to geocentric
double E, N, U;
origin_ENU.Forward(lat, lon, h, E, N, U);
// Factor
Key key1(1), key2(2);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactorArmCalib factor(key1, key2, Point3(E, N, U), model);
// Create a linearization point at zero error
const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45);
const Point3 np = Point3(E, N, U) - nRb * leverArm;
Pose3 T(nRb, np);
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T, leverArm),1e-5));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
[&factor, &leverArm](const Pose3& pose_arg) {
return factor.evaluateError(pose_arg, leverArm);
},
T);
Matrix expectedH2 = numericalDerivative11<Vector, Point3>(
[&factor, &T](const Point3& point_arg) {
return factor.evaluateError(T, point_arg);
},
leverArm);
// Use the factor to calculate the derivative
Matrix actualH1, actualH2;
factor.evaluateError(T, leverArm, actualH1, actualH2);
// Verify we get the expected error
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
}
// ************************************************************************* // *************************************************************************
TEST( GPSFactor2, Constructor ) { TEST( GPSFactor2, Constructor ) {
using namespace example; using namespace example;
@ -174,6 +214,46 @@ TEST( GPSFactor2Arm, Constructor ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-8));
} }
// *************************************************************************
TEST( GPSFactor2ArmCalib, Constructor ) {
using namespace example;
// From lat-lon to geocentric
double E, N, U;
origin_ENU.Forward(lat, lon, h, E, N, U);
// Factor
Key key1(1), key2(2);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactor2ArmCalib factor(key1, key2, Point3(E, N, U), model);
// Create a linearization point at zero error
const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45);
const Point3 np = Point3(E, N, U) - nRb * leverArm;
NavState T(nRb, np, Vector3::Zero());
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T, leverArm),1e-5));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, NavState>(
[&factor, &leverArm](const NavState& nav_arg) {
return factor.evaluateError(nav_arg, leverArm);
},
T);
Matrix expectedH2 = numericalDerivative11<Vector, Point3>(
[&factor, &T](const Point3& point_arg) {
return factor.evaluateError(T, point_arg);
},
leverArm);
// Use the factor to calculate the derivative
Matrix actualH1, actualH2;
factor.evaluateError(T, leverArm, actualH1, actualH2);
// Verify we get the expected error
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
}
//*************************************************************************** //***************************************************************************
TEST(GPSData, init) { TEST(GPSData, init) {