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

Everything compiles and runs with derived classes

release/4.3a0
dellaert 2016-05-15 12:52:41 -07:00
parent 308a75e49b
commit cbf062ff32
10 changed files with 206 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -31,22 +31,21 @@ using namespace std;
//------------------------------------------------------------------------------
// Inner class PreintegratedCombinedMeasurements
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::print(
const string& s) const {
PreintegrationBase::print(s);
void PreintegratedCombinedMeasurements::print(const string& s) const {
PreintegrationType::print(s);
cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
}
//------------------------------------------------------------------------------
bool PreintegratedCombinedMeasurements::equals(
const PreintegratedCombinedMeasurements& other, double tol) const {
return PreintegrationBase::equals(other, tol) &&
equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
return PreintegrationType::equals(other, tol)
&& equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
}
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration();
PreintegrationType::resetIntegration();
preintMeasCov_.setZero();
}
@ -68,9 +67,9 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
// Update preintegrated measurements.
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C;
PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
// Update preintegrated measurements covariance: as in [2] we consider a first
// order propagation that can be seen as a prediction phase in an EKF
@ -80,7 +79,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// Single Jacobians to propagate covariance
// TODO(frank): should we not also account for bias on position?
Matrix3 theta_H_biasOmega = - C.topRows<3>();
Matrix3 theta_H_biasOmega = -C.topRows<3>();
Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
// overall Jacobian wrt preintegrated measurements (df/dx)
@ -105,18 +104,18 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// BLOCK DIAGONAL TERMS
D_t_t(&G_measCov_Gt) = dt * iCov;
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc *
(aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) *
(vel_H_biasAcc.transpose());
D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega *
(wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) *
(theta_H_biasOmega.transpose());
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
* (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
* (vel_H_biasAcc.transpose());
D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
* (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
* (theta_H_biasOmega.transpose());
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) *
theta_H_biasOmega.transpose();
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
* theta_H_biasOmega.transpose();
D_v_R(&G_measCov_Gt) = temp;
D_R_v(&G_measCov_Gt) = temp.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
@ -131,7 +130,7 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt,
const bool use2ndOrderIntegration) {
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
biasHat_ = biasHat;
boost::shared_ptr<Params> p = Params::MakeSharedD();
p->gyroscopeCovariance = measuredOmegaCovariance;
@ -148,12 +147,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
//------------------------------------------------------------------------------
// CombinedImuFactor methods
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor(
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const PreintegratedCombinedMeasurements& pim)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j),
_PIM_(pim) {}
CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j,
Key vel_j, Key bias_i, Key bias_j,
const PreintegratedCombinedMeasurements& pim) :
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j), _PIM_(pim) {
}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
@ -195,8 +194,8 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
Matrix93 D_r_vel_i, D_r_vel_j;
// error wrt preintegrated measurements
Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j,
bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);
// if we need the jacobians
@ -250,11 +249,11 @@ CombinedImuFactor::CombinedImuFactor(
const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
const bool use2ndOrderCoriolis)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j),
_PIM_(pim) {
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j),
_PIM_(pim) {
boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
p->n_gravity = n_gravity;
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
@ -263,12 +262,12 @@ CombinedImuFactor::CombinedImuFactor(
}
void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim,
const Vector3& n_gravity,
const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim,
const Vector3& n_gravity,
const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
// use deprecated predict
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
omegaCoriolis, use2ndOrderCoriolis);
@ -277,5 +276,6 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
}
#endif
} /// namespace gtsam
}
/// namespace gtsam

23
gtsam/navigation/CombinedImuFactor.h
View File

@ -22,12 +22,19 @@
#pragma once
/* GTSAM includes */
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/base/Matrix.h>
namespace gtsam {
#ifdef GTSAM_TANGENT_PREINTEGRATION
typedef TangentPreintegration PreintegrationType;
#else
typedef ManifoldPreintegration PreintegrationType;
#endif
/*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
@ -57,7 +64,7 @@ namespace gtsam {
*
* @addtogroup SLAM
*/
class PreintegratedCombinedMeasurements : public PreintegrationBase {
class PreintegratedCombinedMeasurements : public PreintegrationType {
public:
@ -123,7 +130,7 @@ public:
PreintegratedCombinedMeasurements(
const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
: PreintegrationBase(p, biasHat) {
: PreintegrationType(p, biasHat) {
preintMeasCov_.setZero();
}
@ -133,10 +140,10 @@ public:
/// @{
/// Re-initialize PreintegratedCombinedMeasurements
void resetIntegration();
void resetIntegration() override;
/// const reference to params, shadows definition in base class
Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
Params& p() const { return *boost::static_pointer_cast<Params>(this->p_);}
/// @}
/// @name Access instance variables
@ -146,7 +153,7 @@ public:
/// @name Testable
/// @{
void print(const std::string& s = "Preintegrated Measurements:") const;
void print(const std::string& s = "Preintegrated Measurements:") const override;
bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const;
/// @}
@ -163,7 +170,7 @@ public:
* frame)
*/
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT);
const Vector3& measuredOmega, const double dt) override;
/// @}
@ -183,7 +190,7 @@ public:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
};

89
gtsam/navigation/ImuFactor.cpp
View File

@ -32,20 +32,20 @@ using namespace std;
// Inner class PreintegratedImuMeasurements
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::print(const string& s) const {
PreintegrationBase::print(s);
PreintegrationType::print(s);
cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl;
}
//------------------------------------------------------------------------------
bool PreintegratedImuMeasurements::equals(
const PreintegratedImuMeasurements& other, double tol) const {
return PreintegrationBase::equals(other, tol)
return PreintegrationType::equals(other, tol)
&& equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
}
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration();
PreintegrationType::resetIntegration();
preintMeasCov_.setZero();
}
@ -53,9 +53,9 @@ void PreintegratedImuMeasurements::resetIntegration() {
void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
// Update preintegrated measurements (also get Jacobian)
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C;
PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
// first order covariance propagation:
// as in [2] we consider a first order propagation that can be seen as a
@ -73,30 +73,31 @@ void PreintegratedImuMeasurements::integrateMeasurement(
preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3
preintMeasCov_.block<3,3>(3,3).noalias() += iCov * dt;
preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt;
}
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredAccs,
const Matrix& measuredOmegas,
const Matrix& dts) {
assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1);
void PreintegratedImuMeasurements::integrateMeasurements(
const Matrix& measuredAccs, const Matrix& measuredOmegas,
const Matrix& dts) {
assert(
measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1);
assert(dts.cols() >= 1);
assert(measuredAccs.cols() == dts.cols());
assert(measuredOmegas.cols() == dts.cols());
size_t n = static_cast<size_t>(dts.cols());
for (size_t j = 0; j < n; j++) {
integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0,j));
integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0, j));
}
}
//------------------------------------------------------------------------------
#ifdef GTSAM_TANGENT_PREINTEGRATION
void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
Matrix9* H1, Matrix9* H2) {
PreintegrationBase::mergeWith(pim12, H1, H2);
void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
Matrix9* H1, Matrix9* H2) {
PreintegrationType::mergeWith(pim12, H1, H2);
preintMeasCov_ =
*H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose();
*H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose();
}
#endif
//------------------------------------------------------------------------------
@ -180,12 +181,12 @@ PreintegratedImuMeasurements ImuFactor::Merge(
const PreintegratedImuMeasurements& pim01,
const PreintegratedImuMeasurements& pim12) {
if (!pim01.matchesParamsWith(pim12))
throw std::domain_error(
"Cannot merge PreintegratedImuMeasurements with different params");
throw std::domain_error(
"Cannot merge PreintegratedImuMeasurements with different params");
if (pim01.params()->body_P_sensor)
throw std::domain_error(
"Cannot merge PreintegratedImuMeasurements with sensor pose yet");
throw std::domain_error(
"Cannot merge PreintegratedImuMeasurements with sensor pose yet");
// the bias for the merged factor will be the bias from 01
PreintegratedImuMeasurements pim02 = pim01;
@ -198,25 +199,25 @@ PreintegratedImuMeasurements ImuFactor::Merge(
//------------------------------------------------------------------------------
ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
const shared_ptr& f12) {
const shared_ptr& f12) {
// IMU bias keys must be the same.
if (f01->key5() != f12->key5())
throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
// expect intermediate pose, velocity keys to matchup.
if (f01->key3() != f12->key1() || f01->key4() != f12->key2())
throw std::domain_error(
"ImuFactor::Merge: intermediate pose, velocity keys need to match up");
throw std::domain_error(
"ImuFactor::Merge: intermediate pose, velocity keys need to match up");
// return new factor
auto pim02 =
Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
return boost::make_shared<ImuFactor>(f01->key1(), // P0
f01->key2(), // V0
f12->key3(), // P2
f12->key4(), // V2
f01->key5(), // B
pim02);
Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
return boost::make_shared<ImuFactor>(f01->key1(),// P0
f01->key2(),// V0
f12->key3(),// P2
f12->key4(),// V2
f01->key5(),// B
pim02);
}
#endif
@ -251,9 +252,11 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
//------------------------------------------------------------------------------
// ImuFactor2 methods
//------------------------------------------------------------------------------
ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, const PreintegratedImuMeasurements& pim)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias),
_PIM_(pim) {}
ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias,
const PreintegratedImuMeasurements& pim) :
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j,
bias), _PIM_(pim) {
}
//------------------------------------------------------------------------------
NonlinearFactor::shared_ptr ImuFactor2::clone() const {
@ -269,9 +272,11 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) {
}
//------------------------------------------------------------------------------
void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2())
<< "," << keyFormatter(this->key3()) << ")\n";
void ImuFactor2::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
<< ")\n";
cout << *this << endl;
}
@ -284,15 +289,15 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const {
}
//------------------------------------------------------------------------------
Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j,
const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
Vector ImuFactor2::evaluateError(const NavState& state_i,
const NavState& state_j,
const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3);
}
//------------------------------------------------------------------------------
}
// namespace gtsam
// namespace gtsam

31
gtsam/navigation/ImuFactor.h
View File

@ -23,11 +23,18 @@
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/base/debug.h>
namespace gtsam {
#ifdef GTSAM_TANGENT_PREINTEGRATION
typedef TangentPreintegration PreintegrationType;
#else
typedef ManifoldPreintegration PreintegrationType;
#endif
/*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating
@ -61,7 +68,7 @@ namespace gtsam {
*
* @addtogroup SLAM
*/
class PreintegratedImuMeasurements: public PreintegrationBase {
class PreintegratedImuMeasurements: public PreintegrationType {
friend class ImuFactor;
friend class ImuFactor2;
@ -85,29 +92,28 @@ public:
*/
PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
PreintegrationBase(p, biasHat) {
PreintegrationType(p, biasHat) {
preintMeasCov_.setZero();
}
/**
* Construct preintegrated directly from members: base class and preintMeasCov
* @param base PreintegrationBase instance
* @param base PreintegrationType instance
* @param preintMeasCov Covariance matrix used in noise model.
*/
PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov)
: PreintegrationBase(base),
PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
: PreintegrationType(base),
preintMeasCov_(preintMeasCov) {
}
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
void print(const std::string& s = "Preintegrated Measurements:") const override;
/// equals
bool equals(const PreintegratedImuMeasurements& expected,
double tol = 1e-9) const;
bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
/// Re-initialize PreintegratedIMUMeasurements
void resetIntegration();
void resetIntegration() override;
/**
* Add a single IMU measurement to the preintegration.
@ -115,7 +121,8 @@ public:
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param dt Time interval between this and the last IMU measurement
*/
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt);
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt) override;
/// Add multiple measurements, in matrix columns
void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
@ -152,7 +159,7 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size()));
}
};

21
gtsam/navigation/ManifoldPreintegration.cpp
View File

@ -26,9 +26,9 @@ using namespace std;
namespace gtsam {
//------------------------------------------------------------------------------
ManifoldPreintegration::ManifoldPreintegration(const boost::shared_ptr<Params>& p,
const Bias& biasHat)
: PreintegrationBase(p, biasHat) {
ManifoldPreintegration::ManifoldPreintegration(
const boost::shared_ptr<Params>& p, const Bias& biasHat) :
PreintegrationBase(p, biasHat) {
resetIntegration();
}
@ -46,8 +46,7 @@ void ManifoldPreintegration::resetIntegration() {
//------------------------------------------------------------------------------
bool ManifoldPreintegration::equals(const ManifoldPreintegration& other,
double tol) const {
return p_->equals(*other.p_, tol)
&& fabs(deltaTij_ - other.deltaTij_) < tol
return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol)
&& deltaXij_.equals(other.deltaXij_, tol)
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
@ -58,9 +57,9 @@ bool ManifoldPreintegration::equals(const ManifoldPreintegration& other,
}
//------------------------------------------------------------------------------
void ManifoldPreintegration::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt,
Matrix9* A, Matrix93* B, Matrix93* C) {
void ManifoldPreintegration::update(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
Matrix93* C) {
// Correct for bias in the sensor frame
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
@ -83,8 +82,8 @@ void ManifoldPreintegration::integrateMeasurement(const Vector3& measuredAcc,
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().isZero())
*C += *B* D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
*C += *B * D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// Update Jacobians
@ -141,4 +140,4 @@ Vector9 ManifoldPreintegration::biasCorrectedDelta(
//------------------------------------------------------------------------------
} // namespace gtsam
}// namespace gtsam

17
gtsam/navigation/ManifoldPreintegration.h
View File

@ -21,7 +21,8 @@
#pragma once
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/PreintegrationBase.h>
namespace gtsam {
@ -72,14 +73,14 @@ public:
/// @name Instance variables access
/// @{
const NavState& deltaXij() const override { return deltaXij_; }
const Rot3& deltaRij() const override { return deltaXij_.attitude(); }
Vector3 deltaPij() const override { return deltaXij_.position().vector(); }
Vector3 deltaVij() const override { return deltaXij_.velocity(); }
NavState deltaXij() const override { return deltaXij_; }
Rot3 deltaRij() const override { return deltaXij_.attitude(); }
Vector3 deltaPij() const override { return deltaXij_.position().vector(); }
Vector3 deltaVij() const override { return deltaXij_.velocity(); }
/// @name Testable
/// @{
bool equals(const ManifoldPreintegration& other, double tol) const override;
bool equals(const ManifoldPreintegration& other, double tol) const;
/// @}
/// @name Main functionality
@ -89,8 +90,8 @@ public:
/// It takes measured quantities in the j frame
/// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
/// NOTE(frank): implementation is different in two versions
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
Matrix9* A, Matrix93* B, Matrix93* C) override;
void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
Matrix9* A, Matrix93* B, Matrix93* C) override;
/// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far

11
gtsam/navigation/PreintegrationBase.cpp
View File

@ -31,7 +31,6 @@ namespace gtsam {
PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
const Bias& biasHat)
: p_(p), biasHat_(biasHat), deltaTij_(0.0) {
resetIntegration();
}
//------------------------------------------------------------------------------
@ -96,6 +95,16 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
return make_pair(correctedAcc, correctedOmega);
}
//------------------------------------------------------------------------------
void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt) {
// NOTE(frank): integrateMeasurement always needs to compute the derivatives,
// even when not of interest to the caller. Provide scratch space here.
Matrix9 A;
Matrix93 B, C;
update(measuredAcc, measuredOmega, dt, &A, &B, &C);
}
//------------------------------------------------------------------------------
NavState PreintegrationBase::predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,

20
gtsam/navigation/PreintegrationBase.h
View File

@ -124,9 +124,9 @@ public:
const imuBias::ConstantBias& biasHat() const { return biasHat_; }
double deltaTij() const { return deltaTij_; }
virtual Vector3 deltaPij() const=0;
virtual Vector3 deltaVij() const=0;
virtual Rot3 deltaRij() const=0;
virtual Vector3 deltaPij() const=0;
virtual Vector3 deltaVij() const=0;
virtual Rot3 deltaRij() const=0;
virtual NavState deltaXij() const=0;
// Exposed for MATLAB
@ -136,8 +136,7 @@ public:
/// @name Testable
/// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
void print(const std::string& s) const;
virtual bool equals(const PreintegrationBase& other, double tol) const = 0;
virtual void print(const std::string& s) const;
/// @}
/// @name Main functionality
@ -155,9 +154,13 @@ public:
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
/// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
virtual void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega,
virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0;
/// Version without derivatives
virtual void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt);
/// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far
virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
@ -182,11 +185,6 @@ public:
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
/** Dummy clone for MATLAB */
virtual boost::shared_ptr<PreintegrationBase> clone() const {
return boost::shared_ptr<PreintegrationBase>();
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{

96
gtsam/navigation/TangentPreintegration.cpp
View File

@ -25,8 +25,8 @@ namespace gtsam {
//------------------------------------------------------------------------------
TangentPreintegration::TangentPreintegration(const boost::shared_ptr<Params>& p,
const Bias& biasHat)
: TangentPreintegration(p,biasHat) {
const Bias& biasHat) :
PreintegrationBase(p, biasHat) {
resetIntegration();
}
@ -41,20 +41,21 @@ void TangentPreintegration::resetIntegration() {
//------------------------------------------------------------------------------
bool TangentPreintegration::equals(const TangentPreintegration& other,
double tol) const {
return p_->equals(*other.p_, tol)
&& fabs(deltaTij_ - other.deltaTij_) < tol
return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
&& equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol)
&& equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol);
&& equal_with_abs_tol(preintegrated_H_biasAcc_,
other.preintegrated_H_biasAcc_, tol)
&& equal_with_abs_tol(preintegrated_H_biasOmega_,
other.preintegrated_H_biasOmega_, tol);
}
//------------------------------------------------------------------------------
// See extensive discussion in ImuFactor.lyx
Vector9 TangentPreintegration::UpdatePreintegrated(
const Vector3& a_body, const Vector3& w_body, double dt,
const Vector9& preintegrated, OptionalJacobian<9, 9> A,
OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) {
Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, double dt, const Vector9& preintegrated,
OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B,
OptionalJacobian<9, 3> C) {
const auto theta = preintegrated.segment<3>(0);
const auto position = preintegrated.segment<3>(3);
const auto velocity = preintegrated.segment<3>(6);
@ -65,27 +66,27 @@ Vector9 TangentPreintegration::UpdatePreintegrated(
// Calculate exact mean propagation
Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space
const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const SO3 R = local.expmap();
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
Vector9 preintegratedPlus;
preintegratedPlus << // new preintegrated vector:
theta + w_tangent* dt, // theta
position + velocity* dt + a_nav* dt22, // position
velocity + a_nav* dt; // velocity
preintegratedPlus << // new preintegrated vector:
theta + w_tangent * dt, // theta
position + velocity * dt + a_nav * dt22, // position
velocity + a_nav * dt; // velocity
if (A) {
// Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
A->setIdentity();
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
}
if (B) {
B->block<3, 3>(0, 0) = Z_3x3;
@ -102,10 +103,9 @@ Vector9 TangentPreintegration::UpdatePreintegrated(
}
//------------------------------------------------------------------------------
void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega,
const double dt, Matrix9* A,
Matrix93* B, Matrix93* C) {
void TangentPreintegration::update(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
Matrix93* C) {
// Correct for bias in the sensor frame
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
@ -124,8 +124,8 @@ void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc,
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().isZero())
*C += *B* D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
*C += *B * D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias
@ -135,24 +135,15 @@ void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc,
preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
}
void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega,
double dt) {
// NOTE(frank): integrateMeasurement always needs to compute the derivatives,
// even when not of interest to the caller. Provide scratch space here.
Matrix9 A;
Matrix93 B, C;
integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
}
//------------------------------------------------------------------------------
Vector9 TangentPreintegration::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
// We correct for a change between bias_i and the biasHat_ used to integrate
// This is a simple linear correction with obvious derivatives
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
const Vector9 biasCorrected =
preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() +
preintegrated_H_biasOmega_ * biasIncr.gyroscope();
const Vector9 biasCorrected = preintegrated()
+ preintegrated_H_biasAcc_ * biasIncr.accelerometer()
+ preintegrated_H_biasOmega_ * biasIncr.gyroscope();
if (H) {
(*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_;
@ -174,9 +165,8 @@ Vector9 TangentPreintegration::biasCorrectedDelta(
//------------------------------------------------------------------------------
Vector9 TangentPreintegration::Compose(const Vector9& zeta01,
const Vector9& zeta12, double deltaT12,
OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H2) {
const Vector9& zeta12, double deltaT12, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H2) {
const auto t01 = zeta01.segment<3>(0);
const auto p01 = zeta01.segment<3>(3);
const auto v01 = zeta01.segment<3>(6);
@ -195,9 +185,9 @@ Vector9 TangentPreintegration::Compose(const Vector9& zeta01,
Matrix3 t02_H_R02;
Vector9 zeta02;
const Matrix3 R = R01.matrix();
zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta
p01 + v01 * deltaT12 + R * p12, // position
v01 + R * v12; // velocity
zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta
p01 + v01 * deltaT12 + R * p12, // position
v01 + R * v12; // velocity
if (H1) {
H1->setIdentity();
@ -218,14 +208,16 @@ Vector9 TangentPreintegration::Compose(const Vector9& zeta01,
}
//------------------------------------------------------------------------------
void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, Matrix9* H1,
Matrix9* H2) {
void TangentPreintegration::mergeWith(const TangentPreintegration& pim12,
Matrix9* H1, Matrix9* H2) {
if (!matchesParamsWith(pim12)) {
throw std::domain_error("Cannot merge pre-integrated measurements with different params");
throw std::domain_error(
"Cannot merge pre-integrated measurements with different params");
}
if (params()->body_P_sensor) {
throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet");
throw std::domain_error(
"Cannot merge pre-integrated measurements with sensor pose yet");
}
const double t01 = deltaTij();
@ -241,13 +233,13 @@ void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, Matrix
preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2);
preintegrated_H_biasAcc_ =
(*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_;
preintegrated_H_biasAcc_ = (*H1) * preintegrated_H_biasAcc_
+ (*H2) * pim12.preintegrated_H_biasAcc_;
preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ +
(*H2) * pim12.preintegrated_H_biasOmega_;
preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_
+ (*H2) * pim12.preintegrated_H_biasOmega_;
}
//------------------------------------------------------------------------------
} // namespace gtsam
}// namespace gtsam

15
gtsam/navigation/TangentPreintegration.h
View File

@ -64,9 +64,9 @@ public:
/// @name Instance variables access
/// @{
Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); }
Vector3 deltaVij() const override { return preintegrated_.tail<3>(); }
Rot3 deltaRij() const override { return Rot3::Expmap(theta()); }
Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); }
Vector3 deltaVij() const override { return preintegrated_.tail<3>(); }
Rot3 deltaRij() const override { return Rot3::Expmap(theta()); }
NavState deltaXij() const override { return NavState::Retract(preintegrated_); }
const Vector9& preintegrated() const { return preintegrated_; }
@ -76,7 +76,7 @@ public:
/// @name Testable
/// @{
bool equals(const TangentPreintegration& other, double tol) const override;
bool equals(const TangentPreintegration& other, double tol) const;
/// @}
/// @name Main functionality
@ -91,15 +91,12 @@ public:
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none);
// Version without derivatives
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt);
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
/// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
/// NOTE(frank): implementation is different in two versions
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
Matrix9* A, Matrix93* B, Matrix93* C) override;
void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override;
/// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far