Everything compiles and runs with derived classes
dellaert
parent
308a75e49b
commit
cbf062ff32
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
@ -70,7 +69,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
|
|||
// Update preintegrated measurements.
|
||||
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;
|
||||
|
|
@ -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,9 +249,9 @@ 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,
|
||||
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
|
||||
pose_j, vel_j, bias_i, bias_j),
|
||||
_PIM_(pim) {
|
||||
_PIM_(pim) {
|
||||
boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
|
||||
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
|
||||
p->n_gravity = n_gravity;
|
||||
|
|
@ -277,5 +276,6 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
|||
}
|
||||
#endif
|
||||
|
||||
} /// namespace gtsam
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -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_);
|
||||
}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
@ -55,7 +55,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
|
|||
// Update preintegrated measurements (also get Jacobian)
|
||||
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,20 +73,21 @@ 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,
|
||||
void PreintegratedImuMeasurements::integrateMeasurements(
|
||||
const Matrix& measuredAccs, const Matrix& measuredOmegas,
|
||||
const Matrix& dts) {
|
||||
assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1);
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -94,7 +95,7 @@ void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredA
|
|||
#ifdef GTSAM_TANGENT_PREINTEGRATION
|
||||
void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
|
||||
Matrix9* H1, Matrix9* H2) {
|
||||
PreintegrationBase::mergeWith(pim12, H1, H2);
|
||||
PreintegrationType::mergeWith(pim12, H1, H2);
|
||||
preintMeasCov_ =
|
||||
*H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose();
|
||||
}
|
||||
|
|
@ -211,11 +212,11 @@ ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
|
|||
// 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
|
||||
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,10 +289,10 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const {
|
|||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j,
|
||||
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&> H1, boost::optional<Matrix&> H2,
|
||||
boost::optional<Matrix&> H3) const {
|
||||
return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3);
|
||||
}
|
||||
|
|
@ -295,4 +300,4 @@ Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_
|
|||
//------------------------------------------------------------------------------
|
||||
|
||||
}
|
||||
// namespace gtsam
|
||||
// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -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()));
|
||||
}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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,7 +82,7 @@ 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;
|
||||
*C += *B * D_correctedAcc_omega;
|
||||
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
|
||||
}
|
||||
|
||||
|
|
@ -141,4 +140,4 @@ Vector9 ManifoldPreintegration::biasCorrectedDelta(
|
|||
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
} // namespace gtsam
|
||||
}// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -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(); }
|
||||
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,7 +90,7 @@ 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,
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
@ -73,9 +74,9 @@ Vector9 TangentPreintegration::UpdatePreintegrated(
|
|||
|
||||
Vector9 preintegratedPlus;
|
||||
preintegratedPlus << // new preintegrated vector:
|
||||
theta + w_tangent* dt, // theta
|
||||
position + velocity* dt + a_nav* dt22, // position
|
||||
velocity + a_nav* dt; // velocity
|
||||
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:
|
||||
|
|
@ -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,7 +124,7 @@ 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;
|
||||
*C += *B * D_correctedAcc_omega;
|
||||
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
|
||||
}
|
||||
|
||||
|
|
@ -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,8 +165,7 @@ Vector9 TangentPreintegration::biasCorrectedDelta(
|
|||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector9 TangentPreintegration::Compose(const Vector9& zeta01,
|
||||
const Vector9& zeta12, double deltaT12,
|
||||
OptionalJacobian<9, 9> H1,
|
||||
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);
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue