adding GPSFactors with lever arm as state
morten
parent
0c13038ed5
commit
d1106ae4a3
|
|
@ -95,6 +95,38 @@ Vector GPSFactorArm::evaluateError(const Pose3& p,
|
|||
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 {
|
||||
cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n";
|
||||
|
|
@ -146,4 +178,37 @@ Vector GPSFactor2Arm::evaluateError(const NavState& p,
|
|||
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
* 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
|
||||
|
|
|
|||
|
|
@ -114,6 +114,46 @@ TEST( GPSFactorArm, Constructor ) {
|
|||
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 ) {
|
||||
using namespace example;
|
||||
|
|
@ -174,6 +214,46 @@ TEST( GPSFactor2Arm, Constructor ) {
|
|||
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) {
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue