Frank Dellaert
GitHub
commit
717e3bea63
|
|
@ -64,6 +64,69 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
|
|||
|
||||
return make_pair(nTb, nV);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
void GPSFactorArm::print(const string& s, const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "GPSFactorArm on " << keyFormatter(key()) << "\n";
|
||||
cout << " GPS measurement: " << nT_.transpose() << "\n";
|
||||
cout << " Lever arm: " << bL_.transpose() << "\n";
|
||||
noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
bool GPSFactorArm::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) &&
|
||||
traits<Point3>::Equals(bL_, e->bL_, tol);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
Vector GPSFactorArm::evaluateError(const Pose3& p,
|
||||
OptionalMatrixType H) const {
|
||||
const Matrix3 nRb = p.rotation().matrix();
|
||||
if (H) {
|
||||
H->resize(3, 6);
|
||||
|
||||
H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_);
|
||||
H->block<3, 3>(0, 3) = nRb;
|
||||
}
|
||||
|
||||
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";
|
||||
|
|
@ -85,5 +148,67 @@ Vector GPSFactor2::evaluateError(const NavState& p,
|
|||
}
|
||||
|
||||
//***************************************************************************
|
||||
void GPSFactor2Arm::print(const string& s, const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "GPSFactor2Arm on " << keyFormatter(key()) << "\n";
|
||||
cout << " GPS measurement: " << nT_.transpose() << "\n";
|
||||
cout << " Lever arm: " << bL_.transpose() << "\n";
|
||||
noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
bool GPSFactor2Arm::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) &&
|
||||
traits<Point3>::Equals(bL_, e->bL_, tol);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
Vector GPSFactor2Arm::evaluateError(const NavState& p,
|
||||
OptionalMatrixType H) const {
|
||||
const Matrix3 nRb = p.attitude().matrix();
|
||||
if (H) {
|
||||
H->resize(3, 9);
|
||||
|
||||
H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_);
|
||||
H->block<3, 3>(0, 3) = nRb;
|
||||
H->block<3, 3>(0, 6).setZero();
|
||||
}
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@ namespace gtsam {
|
|||
|
||||
/**
|
||||
* Prior on position in a Cartesian frame.
|
||||
* If there exists a non-zero lever arm between body frame and GPS
|
||||
* antenna, instead use GPSFactorArm.
|
||||
* Possibilities include:
|
||||
* ENU: East-North-Up navigation frame at some local origin
|
||||
* NED: North-East-Down navigation frame at some local origin
|
||||
|
|
@ -38,7 +40,7 @@ private:
|
|||
|
||||
typedef NoiseModelFactorN<Pose3> Base;
|
||||
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -83,6 +85,7 @@ public:
|
|||
/// vector of errors
|
||||
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
|
||||
|
||||
/// return the measurement, in the navigation frame
|
||||
inline const Point3 & measurementIn() const {
|
||||
return nT_;
|
||||
}
|
||||
|
|
@ -112,7 +115,146 @@ private:
|
|||
};
|
||||
|
||||
/**
|
||||
* Version of GPSFactor for NavState
|
||||
* Version of GPSFactor (for Pose3) with 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 GPSFactorArm: public NoiseModelFactorN<Pose3> {
|
||||
|
||||
private:
|
||||
|
||||
typedef NoiseModelFactorN<Pose3> Base;
|
||||
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
|
||||
Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D
|
||||
///< position of the GPS antenna in the body 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<GPSFactorArm> shared_ptr;
|
||||
|
||||
/// Typedef to this class
|
||||
typedef GPSFactorArm This;
|
||||
|
||||
/// default constructor - only use for serialization
|
||||
GPSFactorArm():nT_(0, 0, 0), bL_(0, 0, 0) {}
|
||||
|
||||
~GPSFactorArm() override {}
|
||||
|
||||
/** Constructor from a measurement in a Cartesian frame.
|
||||
* @param key key of the Pose3 variable related to this measurement
|
||||
* @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
|
||||
*/
|
||||
GPSFactorArm(Key key, const Point3& gpsIn, const Point3& leverArm, const SharedNoiseModel& model) :
|
||||
Base(model, key), nT_(gpsIn), bL_(leverArm) {
|
||||
}
|
||||
|
||||
/// @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, OptionalMatrixType H) const override;
|
||||
|
||||
/// return the measurement, in the navigation frame
|
||||
inline const Point3 & measurementIn() const {
|
||||
return nT_;
|
||||
}
|
||||
|
||||
/// return the lever arm, a position in the body frame
|
||||
inline const Point3 & leverArm() const {
|
||||
return bL_;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* 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
|
||||
* antenna, instead use GPSFactor2Arm.
|
||||
* @ingroup navigation
|
||||
*/
|
||||
class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
|
||||
|
|
@ -121,7 +263,7 @@ private:
|
|||
|
||||
typedef NoiseModelFactorN<NavState> Base;
|
||||
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -139,7 +281,11 @@ public:
|
|||
|
||||
~GPSFactor2() override {}
|
||||
|
||||
/// Constructor from a measurement in a Cartesian frame.
|
||||
/** Constructor from a measurement in a Cartesian frame.
|
||||
* @param key key of the NavState variable related to this measurement
|
||||
* @param gpsIn gps measurement, in Cartesian navigation frame
|
||||
* @param model Gaussian noise model
|
||||
*/
|
||||
GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
|
||||
Base(model, key), nT_(gpsIn) {
|
||||
}
|
||||
|
|
@ -160,6 +306,7 @@ public:
|
|||
/// vector of errors
|
||||
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
|
||||
|
||||
/// return the measurement, in the navigation frame
|
||||
inline const Point3 & measurementIn() const {
|
||||
return nT_;
|
||||
}
|
||||
|
|
@ -180,4 +327,140 @@ private:
|
|||
#endif
|
||||
};
|
||||
|
||||
/**
|
||||
* Version of GPSFactor2 with 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 GPSFactor2Arm: public NoiseModelFactorN<NavState> {
|
||||
|
||||
private:
|
||||
|
||||
typedef NoiseModelFactorN<NavState> Base;
|
||||
|
||||
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
|
||||
Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D
|
||||
///< position of the GPS antenna in the body 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<GPSFactor2Arm> shared_ptr;
|
||||
|
||||
/// Typedef to this class
|
||||
typedef GPSFactor2Arm This;
|
||||
|
||||
/// default constructor - only use for serialization
|
||||
GPSFactor2Arm():nT_(0, 0, 0), bL_(0, 0, 0) {}
|
||||
|
||||
~GPSFactor2Arm() override {}
|
||||
|
||||
/** Constructor from a measurement in a Cartesian frame.
|
||||
* @param key key of the NavState variable related to this measurement
|
||||
* @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 noise model for the factor's residual
|
||||
*/
|
||||
GPSFactor2Arm(Key key, const Point3& gpsIn, const Point3& leverArm, const SharedNoiseModel& model) :
|
||||
Base(model, key), nT_(gpsIn), bL_(leverArm) {
|
||||
}
|
||||
|
||||
/// @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, OptionalMatrixType H) const override;
|
||||
|
||||
/// return the measurement, in the navigation frame
|
||||
inline const Point3 & measurementIn() const {
|
||||
return nT_;
|
||||
}
|
||||
|
||||
/// return the lever arm, a position in the body frame
|
||||
inline const Point3 & leverArm() const {
|
||||
return bL_;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* 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
|
||||
|
|
|
|||
|
|
@ -44,7 +44,11 @@ const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
|
|||
LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);
|
||||
|
||||
// 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);
|
||||
|
||||
}
|
||||
|
||||
// *************************************************************************
|
||||
|
|
@ -79,6 +83,77 @@ TEST( GPSFactor, Constructor ) {
|
|||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
// *************************************************************************
|
||||
TEST( GPSFactorArm, Constructor ) {
|
||||
using namespace example;
|
||||
|
||||
// From lat-lon to geocentric
|
||||
double E, N, U;
|
||||
origin_ENU.Forward(lat, lon, h, E, N, U);
|
||||
|
||||
// Factor
|
||||
Key key(1);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
|
||||
GPSFactorArm factor(key, Point3(E, N, U), leverArm, 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),1e-5));
|
||||
|
||||
// Calculate numerical derivatives
|
||||
Matrix expectedH = numericalDerivative11<Vector, Pose3>(
|
||||
[&factor](const Pose3& T) { return factor.evaluateError(T); }, T);
|
||||
|
||||
// Use the factor to calculate the derivative
|
||||
Matrix actualH;
|
||||
factor.evaluateError(T, actualH);
|
||||
|
||||
// Verify we get the expected error
|
||||
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;
|
||||
|
|
@ -108,6 +183,77 @@ TEST( GPSFactor2, Constructor ) {
|
|||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
// *************************************************************************
|
||||
TEST( GPSFactor2Arm, Constructor ) {
|
||||
using namespace example;
|
||||
|
||||
// From lat-lon to geocentric
|
||||
double E, N, U;
|
||||
origin_ENU.Forward(lat, lon, h, E, N, U);
|
||||
|
||||
// Factor
|
||||
Key key(1);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
|
||||
GPSFactor2Arm factor(key, Point3(E, N, U), leverArm, 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),1e-5));
|
||||
|
||||
// Calculate numerical derivatives
|
||||
Matrix expectedH = numericalDerivative11<Vector, NavState>(
|
||||
[&factor](const NavState& T) { return factor.evaluateError(T); }, T);
|
||||
|
||||
// Use the factor to calculate the derivative
|
||||
Matrix actualH;
|
||||
factor.evaluateError(T, actualH);
|
||||
|
||||
// Verify we get the expected error
|
||||
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