Created one base class and two derived classes
dellaert
parent
5b9c966022
commit
308a75e49b
|
|
@ -91,7 +91,7 @@ void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredA
|
|||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
#ifdef GTSAM_TANGENT_PREINTEGRATION
|
||||
void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
|
||||
Matrix9* H1, Matrix9* H2) {
|
||||
PreintegrationBase::mergeWith(pim12, H1, H2);
|
||||
|
|
@ -175,7 +175,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
|
|||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
#ifdef GTSAM_TANGENT_PREINTEGRATION
|
||||
PreintegratedImuMeasurements ImuFactor::Merge(
|
||||
const PreintegratedImuMeasurements& pim01,
|
||||
const PreintegratedImuMeasurements& pim12) {
|
||||
|
|
|
|||
|
|
@ -124,7 +124,7 @@ public:
|
|||
/// Return pre-integrated measurement covariance
|
||||
Matrix preintMeasCov() const { return preintMeasCov_; }
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
#ifdef GTSAM_TANGENT_PREINTEGRATION
|
||||
/// Merge in a different set of measurements and update bias derivatives accordingly
|
||||
void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
|
||||
#endif
|
||||
|
|
@ -232,7 +232,7 @@ public:
|
|||
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
|
||||
boost::none, boost::optional<Matrix&> H5 = boost::none) const;
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
#ifdef GTSAM_TANGENT_PREINTEGRATION
|
||||
/// Merge two pre-integrated measurement classes
|
||||
static PreintegratedImuMeasurements Merge(
|
||||
const PreintegratedImuMeasurements& pim01,
|
||||
|
|
|
|||
|
|
@ -0,0 +1,144 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ManifoldPreintegration.cpp
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include "ManifoldPreintegration.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
ManifoldPreintegration::ManifoldPreintegration(const boost::shared_ptr<Params>& p,
|
||||
const Bias& biasHat)
|
||||
: PreintegrationBase(p, biasHat) {
|
||||
resetIntegration();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void ManifoldPreintegration::resetIntegration() {
|
||||
deltaTij_ = 0.0;
|
||||
deltaXij_ = NavState();
|
||||
delRdelBiasOmega_.setZero();
|
||||
delPdelBiasAcc_.setZero();
|
||||
delPdelBiasOmega_.setZero();
|
||||
delVdelBiasAcc_.setZero();
|
||||
delVdelBiasOmega_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool ManifoldPreintegration::equals(const ManifoldPreintegration& other,
|
||||
double tol) const {
|
||||
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)
|
||||
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void ManifoldPreintegration::integrateMeasurement(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);
|
||||
|
||||
// Possibly correct for sensor pose
|
||||
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
|
||||
if (p().body_P_sensor)
|
||||
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
|
||||
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
|
||||
|
||||
// Save current rotation for updating Jacobians
|
||||
const Rot3 oldRij = deltaXij_.attitude();
|
||||
|
||||
// Do update
|
||||
deltaTij_ += dt;
|
||||
deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional
|
||||
|
||||
if (p().body_P_sensor) {
|
||||
// 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
|
||||
}
|
||||
|
||||
// Update Jacobians
|
||||
// TODO(frank): Try same simplification as in new approach
|
||||
Matrix3 D_acc_R;
|
||||
oldRij.rotate(acc, D_acc_R);
|
||||
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
|
||||
|
||||
const Vector3 integratedOmega = omega * dt;
|
||||
Matrix3 D_incrR_integratedOmega;
|
||||
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
|
||||
const Matrix3 incrRt = incrR.transpose();
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt;
|
||||
|
||||
double dt22 = 0.5 * dt * dt;
|
||||
const Matrix3 dRij = oldRij.matrix(); // expensive
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
|
||||
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
|
||||
delVdelBiasAcc_ += -dRij * dt;
|
||||
delVdelBiasOmega_ += D_acc_biasOmega * dt;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector9 ManifoldPreintegration::biasCorrectedDelta(
|
||||
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
|
||||
// Correct deltaRij, derivative is delRdelBiasOmega_
|
||||
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
|
||||
Matrix3 D_correctedRij_bias;
|
||||
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
|
||||
const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none,
|
||||
H ? &D_correctedRij_bias : 0);
|
||||
if (H)
|
||||
D_correctedRij_bias *= delRdelBiasOmega_;
|
||||
|
||||
Vector9 xi;
|
||||
Matrix3 D_dR_correctedRij;
|
||||
// TODO(frank): could line below be simplified? It is equivalent to
|
||||
// LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
|
||||
NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
|
||||
NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
|
||||
+ delPdelBiasOmega_ * biasIncr.gyroscope();
|
||||
NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer()
|
||||
+ delVdelBiasOmega_ * biasIncr.gyroscope();
|
||||
|
||||
if (H) {
|
||||
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
|
||||
D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias;
|
||||
D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
|
||||
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
|
||||
(*H) << D_dR_bias, D_dP_bias, D_dV_bias;
|
||||
}
|
||||
return xi;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,126 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ManifoldPreintegration.h
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/navigation/ManifoldPreintegration.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* IMU pre-integration on NavSatet manifold.
|
||||
* This corresponds to the original RSS paper (with one difference: V is rotated)
|
||||
*/
|
||||
class ManifoldPreintegration : public PreintegrationBase {
|
||||
protected:
|
||||
|
||||
/**
|
||||
* Pre-integrated navigation state, from frame i to frame j
|
||||
* Note: relative position does not take into account velocity at time i, see deltap+, in [2]
|
||||
* Note: velocity is now also in frame i, as opposed to deltaVij in [2]
|
||||
*/
|
||||
NavState deltaXij_;
|
||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
|
||||
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
|
||||
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
|
||||
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
|
||||
|
||||
/// Default constructor for serialization
|
||||
ManifoldPreintegration() {
|
||||
resetIntegration();
|
||||
}
|
||||
|
||||
public:
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Constructor, initializes the variables in the base class
|
||||
* @param p Parameters, typically fixed in a single application
|
||||
* @param bias Current estimate of acceleration and rotation rate biases
|
||||
*/
|
||||
ManifoldPreintegration(const boost::shared_ptr<Params>& p,
|
||||
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Basic utilities
|
||||
/// @{
|
||||
/// Re-initialize PreintegratedMeasurements
|
||||
void resetIntegration() override;
|
||||
|
||||
/// @}
|
||||
|
||||
/// @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(); }
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
bool equals(const ManifoldPreintegration& other, double tol) const override;
|
||||
/// @}
|
||||
|
||||
/// @name Main functionality
|
||||
/// @{
|
||||
|
||||
/// 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;
|
||||
|
||||
/// Given the estimate of the bias, return a NavState tangent vector
|
||||
/// summarizing the preintegrated IMU measurements so far
|
||||
/// NOTE(frank): implementation is different in two versions
|
||||
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
|
||||
OptionalJacobian<9, 6> H = boost::none) const override;
|
||||
|
||||
/** Dummy clone for MATLAB */
|
||||
virtual boost::shared_ptr<ManifoldPreintegration> clone() const {
|
||||
return boost::shared_ptr<ManifoldPreintegration>();
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
namespace bs = ::boost::serialization;
|
||||
ar & BOOST_SERIALIZATION_NVP(p_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasHat_);
|
||||
ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
|
||||
ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
|
||||
ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
|
||||
ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
|
||||
ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
|
||||
}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -239,7 +239,7 @@ Matrix7 NavState::wedge(const Vector9& xi) {
|
|||
#define D_v_v(H) (H)->block<3,3>(6,6)
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_IMU_MANIFOLD_INTEGRATION)
|
||||
#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_TANGENT_PREINTEGRATION)
|
||||
NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
|
||||
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
|
||||
OptionalJacobian<9, 3> G2) const {
|
||||
|
|
|
|||
|
|
@ -203,7 +203,7 @@ public:
|
|||
/// @name Dynamics
|
||||
/// @{
|
||||
|
||||
#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_IMU_MANIFOLD_INTEGRATION)
|
||||
#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_TANGENT_PREINTEGRATION)
|
||||
/// Integrate forward in time given angular velocity and acceleration in body frame
|
||||
/// Uses second order integration for position, returns derivatives except dt.
|
||||
NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
|
||||
|
|
|
|||
|
|
@ -34,31 +34,10 @@ PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
|
|||
resetIntegration();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegrationBase::resetIntegration() {
|
||||
deltaTij_ = 0.0;
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
preintegrated_.setZero();
|
||||
preintegrated_H_biasAcc_.setZero();
|
||||
preintegrated_H_biasOmega_.setZero();
|
||||
#else
|
||||
deltaXij_ = NavState();
|
||||
delRdelBiasOmega_.setZero();
|
||||
delPdelBiasAcc_.setZero();
|
||||
delPdelBiasOmega_.setZero();
|
||||
delVdelBiasAcc_.setZero();
|
||||
delVdelBiasOmega_.setZero();
|
||||
#endif
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
ostream& operator<<(ostream& os, const PreintegrationBase& pim) {
|
||||
os << " deltaTij " << pim.deltaTij_ << endl;
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
os << " deltaRij " << Point3(pim.theta()) << endl;
|
||||
#else
|
||||
os << " deltaRij.ypr = (" << pim.deltaRij().ypr().transpose() << ")" << endl;
|
||||
#endif
|
||||
os << " deltaPij " << Point3(pim.deltaPij()) << endl;
|
||||
os << " deltaVij " << Point3(pim.deltaVij()) << endl;
|
||||
os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl;
|
||||
|
|
@ -71,26 +50,6 @@ void PreintegrationBase::print(const string& s) const {
|
|||
cout << s << *this << endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool PreintegrationBase::equals(const PreintegrationBase& other,
|
||||
double tol) const {
|
||||
return p_->equals(*other.p_, tol)
|
||||
&& fabs(deltaTij_ - other.deltaTij_) < tol
|
||||
&& biasHat_.equals(other.biasHat_, tol)
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
&& 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);
|
||||
#else
|
||||
&& deltaXij_.equals(other.deltaXij_, tol)
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
|
||||
#endif
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
|
||||
const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
|
||||
|
|
@ -137,203 +96,6 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
|
|||
return make_pair(correctedAcc, correctedOmega);
|
||||
}
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
//------------------------------------------------------------------------------
|
||||
// See extensive discussion in ImuFactor.lyx
|
||||
Vector9 PreintegrationBase::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);
|
||||
|
||||
// This functor allows for saving computation when exponential map and its
|
||||
// derivatives are needed at the same location in so<3>
|
||||
so3::DexpFunctor local(theta);
|
||||
|
||||
// Calculate exact mean propagation
|
||||
Matrix3 w_tangent_H_theta, invH;
|
||||
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
|
||||
|
||||
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
|
||||
}
|
||||
if (B) {
|
||||
B->block<3, 3>(0, 0) = Z_3x3;
|
||||
B->block<3, 3>(3, 0) = R * dt22;
|
||||
B->block<3, 3>(6, 0) = R * dt;
|
||||
}
|
||||
if (C) {
|
||||
C->block<3, 3>(0, 0) = invH * dt;
|
||||
C->block<3, 3>(3, 0) = Z_3x3;
|
||||
C->block<3, 3>(6, 0) = Z_3x3;
|
||||
}
|
||||
|
||||
return preintegratedPlus;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegrationBase::integrateMeasurement(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);
|
||||
|
||||
// Possibly correct for sensor pose
|
||||
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
|
||||
if (p().body_P_sensor)
|
||||
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
|
||||
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
|
||||
|
||||
// Do update
|
||||
deltaTij_ += dt;
|
||||
preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);
|
||||
|
||||
if (p().body_P_sensor) {
|
||||
// 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
|
||||
}
|
||||
|
||||
// D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias
|
||||
preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);
|
||||
|
||||
// D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias
|
||||
preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
|
||||
}
|
||||
|
||||
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;
|
||||
integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
Vector9 PreintegrationBase::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();
|
||||
|
||||
if (H) {
|
||||
(*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_;
|
||||
}
|
||||
return biasCorrected;
|
||||
}
|
||||
#else
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegrationBase::integrateMeasurement(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);
|
||||
|
||||
// Possibly correct for sensor pose
|
||||
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
|
||||
if (p().body_P_sensor)
|
||||
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
|
||||
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
|
||||
|
||||
// Save current rotation for updating Jacobians
|
||||
const Rot3 oldRij = deltaXij_.attitude();
|
||||
|
||||
// Do update
|
||||
deltaTij_ += dt;
|
||||
deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional
|
||||
|
||||
if (p().body_P_sensor) {
|
||||
// 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
|
||||
}
|
||||
|
||||
// Update Jacobians
|
||||
// TODO(frank): Try same simplification as in new approach
|
||||
Matrix3 D_acc_R;
|
||||
oldRij.rotate(acc, D_acc_R);
|
||||
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
|
||||
|
||||
const Vector3 integratedOmega = omega * dt;
|
||||
Matrix3 D_incrR_integratedOmega;
|
||||
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
|
||||
const Matrix3 incrRt = incrR.transpose();
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt;
|
||||
|
||||
double dt22 = 0.5 * dt * dt;
|
||||
const Matrix3 dRij = oldRij.matrix(); // expensive
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
|
||||
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
|
||||
delVdelBiasAcc_ += -dRij * dt;
|
||||
delVdelBiasOmega_ += D_acc_biasOmega * dt;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector9 PreintegrationBase::biasCorrectedDelta(
|
||||
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
|
||||
// Correct deltaRij, derivative is delRdelBiasOmega_
|
||||
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
|
||||
Matrix3 D_correctedRij_bias;
|
||||
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
|
||||
const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none,
|
||||
H ? &D_correctedRij_bias : 0);
|
||||
if (H)
|
||||
D_correctedRij_bias *= delRdelBiasOmega_;
|
||||
|
||||
Vector9 xi;
|
||||
Matrix3 D_dR_correctedRij;
|
||||
// TODO(frank): could line below be simplified? It is equivalent to
|
||||
// LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
|
||||
NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
|
||||
NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
|
||||
+ delPdelBiasOmega_ * biasIncr.gyroscope();
|
||||
NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer()
|
||||
+ delVdelBiasOmega_ * biasIncr.gyroscope();
|
||||
|
||||
if (H) {
|
||||
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
|
||||
D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias;
|
||||
D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
|
||||
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
|
||||
(*H) << D_dR_bias, D_dP_bias, D_dV_bias;
|
||||
}
|
||||
return xi;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
NavState PreintegrationBase::predict(const NavState& state_i,
|
||||
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
|
||||
|
|
@ -415,96 +177,6 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
|
|||
return error;
|
||||
}
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
//------------------------------------------------------------------------------
|
||||
// sugar for derivative blocks
|
||||
#define D_R_R(H) (H)->block<3,3>(0,0)
|
||||
#define D_R_t(H) (H)->block<3,3>(0,3)
|
||||
#define D_R_v(H) (H)->block<3,3>(0,6)
|
||||
#define D_t_R(H) (H)->block<3,3>(3,0)
|
||||
#define D_t_t(H) (H)->block<3,3>(3,3)
|
||||
#define D_t_v(H) (H)->block<3,3>(3,6)
|
||||
#define D_v_R(H) (H)->block<3,3>(6,0)
|
||||
#define D_v_t(H) (H)->block<3,3>(6,3)
|
||||
#define D_v_v(H) (H)->block<3,3>(6,6)
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector9 PreintegrationBase::Compose(const Vector9& zeta01,
|
||||
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);
|
||||
|
||||
const auto t12 = zeta12.segment<3>(0);
|
||||
const auto p12 = zeta12.segment<3>(3);
|
||||
const auto v12 = zeta12.segment<3>(6);
|
||||
|
||||
Matrix3 R01_H_t01, R12_H_t12;
|
||||
const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01);
|
||||
const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12);
|
||||
|
||||
Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity
|
||||
const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12);
|
||||
|
||||
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
|
||||
|
||||
if (H1) {
|
||||
H1->setIdentity();
|
||||
D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01;
|
||||
D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01;
|
||||
D_t_v(H1) = I_3x3 * deltaT12;
|
||||
D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01;
|
||||
}
|
||||
|
||||
if (H2) {
|
||||
H2->setZero();
|
||||
D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12;
|
||||
D_t_t(H2) = R;
|
||||
D_v_v(H2) = R;
|
||||
}
|
||||
|
||||
return zeta02;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
|
||||
Matrix9* H2) {
|
||||
if (!matchesParamsWith(pim12)) {
|
||||
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");
|
||||
}
|
||||
|
||||
const double t01 = deltaTij();
|
||||
const double t12 = pim12.deltaTij();
|
||||
deltaTij_ = t01 + t12;
|
||||
|
||||
const Vector9 zeta01 = preintegrated();
|
||||
Vector9 zeta12 = pim12.preintegrated(); // will be modified.
|
||||
|
||||
const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
|
||||
zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
|
||||
+ pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer();
|
||||
|
||||
preintegrated_ = PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2);
|
||||
|
||||
preintegrated_H_biasAcc_ =
|
||||
(*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_;
|
||||
|
||||
preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ +
|
||||
(*H2) * pim12.preintegrated_H_biasOmega_;
|
||||
}
|
||||
#endif
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
|
||||
|
|
|
|||
|
|
@ -75,37 +75,13 @@ class PreintegrationBase {
|
|||
/// Time interval from i to j
|
||||
double deltaTij_;
|
||||
|
||||
/**
|
||||
* Preintegrated navigation state, from frame i to frame j
|
||||
* Order is: theta, position, velocity
|
||||
* Note: relative position does not take into account velocity at time i, see deltap+, in [2]
|
||||
* Note: velocity is now also in frame i, as opposed to deltaVij in [2]
|
||||
*/
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
Vector9 preintegrated_;
|
||||
Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias
|
||||
Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias
|
||||
#else
|
||||
NavState deltaXij_;
|
||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
|
||||
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
|
||||
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
|
||||
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
|
||||
#endif
|
||||
|
||||
/// Default constructor for serialization
|
||||
PreintegrationBase() {
|
||||
resetIntegration();
|
||||
}
|
||||
PreintegrationBase() {}
|
||||
|
||||
public:
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Constructor, initializes the variables in the base class
|
||||
* @param p Parameters, typically fixed in a single application
|
||||
|
|
@ -119,7 +95,7 @@ public:
|
|||
/// @name Basic utilities
|
||||
/// @{
|
||||
/// Re-initialize PreintegratedMeasurements
|
||||
void resetIntegration();
|
||||
virtual void resetIntegration()=0;
|
||||
|
||||
/// check parameters equality: checks whether shared pointer points to same Params object.
|
||||
bool matchesParamsWith(const PreintegrationBase& other) const {
|
||||
|
|
@ -148,22 +124,10 @@ public:
|
|||
const imuBias::ConstantBias& biasHat() const { return biasHat_; }
|
||||
double deltaTij() const { return deltaTij_; }
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
const Vector9& preintegrated() const { return preintegrated_; }
|
||||
Vector3 theta() const { return preintegrated_.head<3>(); }
|
||||
Vector3 deltaPij() const { return preintegrated_.segment<3>(3); }
|
||||
Vector3 deltaVij() const { return preintegrated_.tail<3>(); }
|
||||
Rot3 deltaRij() const { return Rot3::Expmap(theta()); }
|
||||
NavState deltaXij() const { return NavState::Retract(preintegrated_); }
|
||||
|
||||
const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
|
||||
const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
|
||||
#else
|
||||
const NavState& deltaXij() const { return deltaXij_; }
|
||||
const Rot3& deltaRij() const { return deltaXij_.attitude(); }
|
||||
Vector3 deltaPij() const { return deltaXij_.position().vector(); }
|
||||
Vector3 deltaVij() const { return deltaXij_.velocity(); }
|
||||
#endif
|
||||
virtual Vector3 deltaPij() const=0;
|
||||
virtual Vector3 deltaVij() const=0;
|
||||
virtual Rot3 deltaRij() const=0;
|
||||
virtual NavState deltaXij() const=0;
|
||||
|
||||
// Exposed for MATLAB
|
||||
Vector6 biasHatVector() const { return biasHat_.vector(); }
|
||||
|
|
@ -173,7 +137,7 @@ public:
|
|||
/// @{
|
||||
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
|
||||
void print(const std::string& s) const;
|
||||
bool equals(const PreintegrationBase& other, double tol) const;
|
||||
virtual bool equals(const PreintegrationBase& other, double tol) const = 0;
|
||||
/// @}
|
||||
|
||||
/// @name Main functionality
|
||||
|
|
@ -188,34 +152,16 @@ public:
|
|||
OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
|
||||
OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
|
||||
// Update integrated vector on tangent manifold preintegrated with acceleration
|
||||
// Static, functional version.
|
||||
static Vector9 UpdatePreintegrated(const Vector3& a_body,
|
||||
const Vector3& w_body, const double dt,
|
||||
const Vector9& preintegrated,
|
||||
OptionalJacobian<9, 9> A = boost::none,
|
||||
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);
|
||||
|
||||
#endif
|
||||
|
||||
/// 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);
|
||||
virtual void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega,
|
||||
const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0;
|
||||
|
||||
/// Given the estimate of the bias, return a NavState tangent vector
|
||||
/// summarizing the preintegrated IMU measurements so far
|
||||
/// NOTE(frank): implementation is different in two versions
|
||||
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
|
||||
OptionalJacobian<9, 6> H = boost::none) const;
|
||||
virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
|
||||
OptionalJacobian<9, 6> H = boost::none) const=0;
|
||||
|
||||
/// Predict state at time j
|
||||
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
|
||||
|
|
@ -236,19 +182,6 @@ public:
|
|||
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
|
||||
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
// Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
|
||||
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
|
||||
double deltaT12,
|
||||
OptionalJacobian<9, 9> H1 = boost::none,
|
||||
OptionalJacobian<9, 9> H2 = boost::none);
|
||||
|
||||
/// Merge in a different set of measurements and update bias derivatives accordingly
|
||||
/// The derivatives apply to the preintegrated Vector9
|
||||
void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2);
|
||||
/// @}
|
||||
#endif
|
||||
|
||||
/** Dummy clone for MATLAB */
|
||||
virtual boost::shared_ptr<PreintegrationBase> clone() const {
|
||||
return boost::shared_ptr<PreintegrationBase>();
|
||||
|
|
@ -267,31 +200,6 @@ public:
|
|||
#endif
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
namespace bs = ::boost::serialization;
|
||||
ar & BOOST_SERIALIZATION_NVP(p_);
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
ar & BOOST_SERIALIZATION_NVP(biasHat_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||
ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size()));
|
||||
ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
|
||||
ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
|
||||
#else
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasHat_);
|
||||
ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
|
||||
ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
|
||||
ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
|
||||
ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
|
||||
ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
|
||||
#endif
|
||||
}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -0,0 +1,253 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file TangentPreintegration.cpp
|
||||
* @author Frank Dellaert
|
||||
* @author Adam Bry
|
||||
**/
|
||||
|
||||
#include "TangentPreintegration.h"
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
TangentPreintegration::TangentPreintegration(const boost::shared_ptr<Params>& p,
|
||||
const Bias& biasHat)
|
||||
: TangentPreintegration(p,biasHat) {
|
||||
resetIntegration();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void TangentPreintegration::resetIntegration() {
|
||||
deltaTij_ = 0.0;
|
||||
preintegrated_.setZero();
|
||||
preintegrated_H_biasAcc_.setZero();
|
||||
preintegrated_H_biasOmega_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool TangentPreintegration::equals(const TangentPreintegration& other,
|
||||
double tol) const {
|
||||
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);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// 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) {
|
||||
const auto theta = preintegrated.segment<3>(0);
|
||||
const auto position = preintegrated.segment<3>(3);
|
||||
const auto velocity = preintegrated.segment<3>(6);
|
||||
|
||||
// This functor allows for saving computation when exponential map and its
|
||||
// derivatives are needed at the same location in so<3>
|
||||
so3::DexpFunctor local(theta);
|
||||
|
||||
// Calculate exact mean propagation
|
||||
Matrix3 w_tangent_H_theta, invH;
|
||||
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
|
||||
|
||||
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
|
||||
}
|
||||
if (B) {
|
||||
B->block<3, 3>(0, 0) = Z_3x3;
|
||||
B->block<3, 3>(3, 0) = R * dt22;
|
||||
B->block<3, 3>(6, 0) = R * dt;
|
||||
}
|
||||
if (C) {
|
||||
C->block<3, 3>(0, 0) = invH * dt;
|
||||
C->block<3, 3>(3, 0) = Z_3x3;
|
||||
C->block<3, 3>(6, 0) = Z_3x3;
|
||||
}
|
||||
|
||||
return preintegratedPlus;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void TangentPreintegration::integrateMeasurement(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);
|
||||
|
||||
// Possibly correct for sensor pose
|
||||
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
|
||||
if (p().body_P_sensor)
|
||||
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
|
||||
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
|
||||
|
||||
// Do update
|
||||
deltaTij_ += dt;
|
||||
preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);
|
||||
|
||||
if (p().body_P_sensor) {
|
||||
// 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
|
||||
}
|
||||
|
||||
// D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias
|
||||
preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);
|
||||
|
||||
// D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias
|
||||
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();
|
||||
|
||||
if (H) {
|
||||
(*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_;
|
||||
}
|
||||
return biasCorrected;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// sugar for derivative blocks
|
||||
#define D_R_R(H) (H)->block<3,3>(0,0)
|
||||
#define D_R_t(H) (H)->block<3,3>(0,3)
|
||||
#define D_R_v(H) (H)->block<3,3>(0,6)
|
||||
#define D_t_R(H) (H)->block<3,3>(3,0)
|
||||
#define D_t_t(H) (H)->block<3,3>(3,3)
|
||||
#define D_t_v(H) (H)->block<3,3>(3,6)
|
||||
#define D_v_R(H) (H)->block<3,3>(6,0)
|
||||
#define D_v_t(H) (H)->block<3,3>(6,3)
|
||||
#define D_v_v(H) (H)->block<3,3>(6,6)
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector9 TangentPreintegration::Compose(const Vector9& zeta01,
|
||||
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);
|
||||
|
||||
const auto t12 = zeta12.segment<3>(0);
|
||||
const auto p12 = zeta12.segment<3>(3);
|
||||
const auto v12 = zeta12.segment<3>(6);
|
||||
|
||||
Matrix3 R01_H_t01, R12_H_t12;
|
||||
const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01);
|
||||
const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12);
|
||||
|
||||
Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity
|
||||
const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12);
|
||||
|
||||
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
|
||||
|
||||
if (H1) {
|
||||
H1->setIdentity();
|
||||
D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01;
|
||||
D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01;
|
||||
D_t_v(H1) = I_3x3 * deltaT12;
|
||||
D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01;
|
||||
}
|
||||
|
||||
if (H2) {
|
||||
H2->setZero();
|
||||
D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12;
|
||||
D_t_t(H2) = R;
|
||||
D_v_v(H2) = R;
|
||||
}
|
||||
|
||||
return zeta02;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
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");
|
||||
}
|
||||
|
||||
if (params()->body_P_sensor) {
|
||||
throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet");
|
||||
}
|
||||
|
||||
const double t01 = deltaTij();
|
||||
const double t12 = pim12.deltaTij();
|
||||
deltaTij_ = t01 + t12;
|
||||
|
||||
const Vector9 zeta01 = preintegrated();
|
||||
Vector9 zeta12 = pim12.preintegrated(); // will be modified.
|
||||
|
||||
const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
|
||||
zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
|
||||
+ pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer();
|
||||
|
||||
preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2);
|
||||
|
||||
preintegrated_H_biasAcc_ =
|
||||
(*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_;
|
||||
|
||||
preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ +
|
||||
(*H2) * pim12.preintegrated_H_biasOmega_;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,143 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file TangentPreintegration.h
|
||||
* @author Frank Dellaert
|
||||
* @author Adam Bry
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/navigation/PreintegrationBase.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Integrate on the 9D tangent space of the NavState manifold.
|
||||
* See extensive discussion in ImuFactor.lyx
|
||||
*/
|
||||
class TangentPreintegration : public PreintegrationBase {
|
||||
protected:
|
||||
|
||||
/**
|
||||
* Preintegrated navigation state, as a 9D vector on tangent space at frame i
|
||||
* Order is: theta, position, velocity
|
||||
*/
|
||||
Vector9 preintegrated_;
|
||||
Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias
|
||||
Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias
|
||||
|
||||
/// Default constructor for serialization
|
||||
TangentPreintegration() {
|
||||
resetIntegration();
|
||||
}
|
||||
|
||||
public:
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Constructor, initializes the variables in the base class
|
||||
* @param p Parameters, typically fixed in a single application
|
||||
* @param bias Current estimate of acceleration and rotation rate biases
|
||||
*/
|
||||
TangentPreintegration(const boost::shared_ptr<Params>& p,
|
||||
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Basic utilities
|
||||
/// @{
|
||||
/// Re-initialize PreintegratedMeasurements
|
||||
void resetIntegration() override;
|
||||
|
||||
/// @}
|
||||
|
||||
/// @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()); }
|
||||
NavState deltaXij() const override { return NavState::Retract(preintegrated_); }
|
||||
|
||||
const Vector9& preintegrated() const { return preintegrated_; }
|
||||
Vector3 theta() const { return preintegrated_.head<3>(); }
|
||||
const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
|
||||
const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
bool equals(const TangentPreintegration& other, double tol) const override;
|
||||
/// @}
|
||||
|
||||
/// @name Main functionality
|
||||
/// @{
|
||||
|
||||
// Update integrated vector on tangent manifold preintegrated with acceleration
|
||||
// Static, functional version.
|
||||
static Vector9 UpdatePreintegrated(const Vector3& a_body,
|
||||
const Vector3& w_body, const double dt,
|
||||
const Vector9& preintegrated,
|
||||
OptionalJacobian<9, 9> A = boost::none,
|
||||
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;
|
||||
|
||||
/// Given the estimate of the bias, return a NavState tangent vector
|
||||
/// summarizing the preintegrated IMU measurements so far
|
||||
/// NOTE(frank): implementation is different in two versions
|
||||
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
|
||||
OptionalJacobian<9, 6> H = boost::none) const override;
|
||||
|
||||
// Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
|
||||
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
|
||||
double deltaT12,
|
||||
OptionalJacobian<9, 9> H1 = boost::none,
|
||||
OptionalJacobian<9, 9> H2 = boost::none);
|
||||
|
||||
/// Merge in a different set of measurements and update bias derivatives accordingly
|
||||
/// The derivatives apply to the preintegrated Vector9
|
||||
void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2);
|
||||
/// @}
|
||||
|
||||
/** Dummy clone for MATLAB */
|
||||
virtual boost::shared_ptr<TangentPreintegration> clone() const {
|
||||
return boost::shared_ptr<TangentPreintegration>();
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
namespace bs = ::boost::serialization;
|
||||
ar & BOOST_SERIALIZATION_NVP(p_);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasHat_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||
ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size()));
|
||||
ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
|
||||
ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
|
||||
}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -132,7 +132,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
#ifdef GTSAM_TANGENT_PREINTEGRATION
|
||||
TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
|
||||
auto p = testing::Params();
|
||||
testing::SomeMeasurements measurements;
|
||||
|
|
|
|||
|
|
@ -445,7 +445,7 @@ TEST(ImuFactor, fistOrderExponential) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
#ifdef GTSAM_TANGENT_PREINTEGRATION
|
||||
TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
|
||||
testing::SomeMeasurements measurements;
|
||||
|
||||
|
|
@ -796,7 +796,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
#ifdef GTSAM_TANGENT_PREINTEGRATION
|
||||
static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6;
|
||||
|
||||
struct ImuFactorMergeTest {
|
||||
|
|
|
|||
|
|
@ -10,12 +10,12 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testPreintegrationBase.cpp
|
||||
* @file testTangentPreintegration.cpp
|
||||
* @brief Unit test for the InertialNavFactor
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/PreintegrationBase.h>
|
||||
#include <gtsam/navigation/TangentPreintegration.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
|
|
@ -26,11 +26,10 @@
|
|||
|
||||
#include "imuFactorTesting.h"
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
static const double kDt = 0.1;
|
||||
|
||||
Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
|
||||
return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta);
|
||||
return TangentPreintegration::UpdatePreintegrated(a, w, kDt, zeta);
|
||||
}
|
||||
|
||||
namespace testing {
|
||||
|
|
@ -45,8 +44,8 @@ static boost::shared_ptr<PreintegrationParams> Params() {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(PreintegrationBase, UpdateEstimate1) {
|
||||
PreintegrationBase pim(testing::Params());
|
||||
TEST(TangentPreintegration, UpdateEstimate1) {
|
||||
TangentPreintegration pim(testing::Params());
|
||||
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
|
||||
Vector9 zeta;
|
||||
zeta.setZero();
|
||||
|
|
@ -59,8 +58,8 @@ TEST(PreintegrationBase, UpdateEstimate1) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(PreintegrationBase, UpdateEstimate2) {
|
||||
PreintegrationBase pim(testing::Params());
|
||||
TEST(TangentPreintegration, UpdateEstimate2) {
|
||||
TangentPreintegration pim(testing::Params());
|
||||
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
|
||||
Vector9 zeta;
|
||||
zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
|
||||
|
|
@ -74,8 +73,8 @@ TEST(PreintegrationBase, UpdateEstimate2) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(PreintegrationBase, computeError) {
|
||||
PreintegrationBase pim(testing::Params());
|
||||
TEST(TangentPreintegration, computeError) {
|
||||
TangentPreintegration pim(testing::Params());
|
||||
NavState x1, x2;
|
||||
imuBias::ConstantBias bias;
|
||||
Matrix9 aH1, aH2;
|
||||
|
|
@ -83,7 +82,7 @@ TEST(PreintegrationBase, computeError) {
|
|||
pim.computeError(x1, x2, bias, aH1, aH2, aH3);
|
||||
boost::function<Vector9(const NavState&, const NavState&,
|
||||
const imuBias::ConstantBias&)> f =
|
||||
boost::bind(&PreintegrationBase::computeError, pim, _1, _2, _3,
|
||||
boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3,
|
||||
boost::none, boost::none, boost::none);
|
||||
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
|
||||
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
|
||||
|
|
@ -92,47 +91,47 @@ TEST(PreintegrationBase, computeError) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(PreintegrationBase, Compose) {
|
||||
TEST(TangentPreintegration, Compose) {
|
||||
testing::SomeMeasurements measurements;
|
||||
PreintegrationBase pim(testing::Params());
|
||||
TangentPreintegration pim(testing::Params());
|
||||
testing::integrateMeasurements(measurements, &pim);
|
||||
|
||||
boost::function<Vector9(const Vector9&, const Vector9&)> f =
|
||||
[pim](const Vector9& zeta01, const Vector9& zeta12) {
|
||||
return PreintegrationBase::Compose(zeta01, zeta12, pim.deltaTij());
|
||||
return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij());
|
||||
};
|
||||
|
||||
// Expected merge result
|
||||
PreintegrationBase expected_pim02(testing::Params());
|
||||
TangentPreintegration expected_pim02(testing::Params());
|
||||
testing::integrateMeasurements(measurements, &expected_pim02);
|
||||
testing::integrateMeasurements(measurements, &expected_pim02);
|
||||
|
||||
// Actual result
|
||||
Matrix9 H1, H2;
|
||||
PreintegrationBase actual_pim02 = pim;
|
||||
TangentPreintegration actual_pim02 = pim;
|
||||
actual_pim02.mergeWith(pim, &H1, &H2);
|
||||
|
||||
const Vector9 zeta = pim.preintegrated();
|
||||
const Vector9 actual_zeta =
|
||||
PreintegrationBase::Compose(zeta, zeta, pim.deltaTij());
|
||||
TangentPreintegration::Compose(zeta, zeta, pim.deltaTij());
|
||||
EXPECT(assert_equal(expected_pim02.preintegrated(), actual_zeta, 1e-7));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(PreintegrationBase, MergedBiasDerivatives) {
|
||||
TEST(TangentPreintegration, MergedBiasDerivatives) {
|
||||
testing::SomeMeasurements measurements;
|
||||
|
||||
auto f = [=](const Vector3& a, const Vector3& w) {
|
||||
PreintegrationBase pim02(testing::Params(), Bias(a, w));
|
||||
TangentPreintegration pim02(testing::Params(), Bias(a, w));
|
||||
testing::integrateMeasurements(measurements, &pim02);
|
||||
testing::integrateMeasurements(measurements, &pim02);
|
||||
return pim02.preintegrated();
|
||||
};
|
||||
|
||||
// Expected merge result
|
||||
PreintegrationBase expected_pim02(testing::Params());
|
||||
TangentPreintegration expected_pim02(testing::Params());
|
||||
testing::integrateMeasurements(measurements, &expected_pim02);
|
||||
testing::integrateMeasurements(measurements, &expected_pim02);
|
||||
|
||||
|
|
@ -141,7 +140,6 @@ TEST(PreintegrationBase, MergedBiasDerivatives) {
|
|||
EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(f, Z_3x1, Z_3x1),
|
||||
expected_pim02.preintegrated_H_biasOmega(), 1e-7));
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
Loading…
Reference in New Issue