Refactor of bias derivatives.
New bias derivatives work for acc but not omegarelease/4.3a0
Frank Dellaert
parent
23e76efdc4
commit
d2d6590854
|
|
@ -20,9 +20,8 @@
|
|||
**/
|
||||
|
||||
#include "PreintegrationBase.h"
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <boost/make_shared.hpp>
|
||||
#endif
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -78,14 +77,14 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
|
|||
|
||||
//------------------------------------------------------------------------------
|
||||
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
|
||||
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega,
|
||||
OptionalJacobian<3, 3> D_correctedAcc_measuredAcc,
|
||||
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
|
||||
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
|
||||
|
||||
// Correct for bias in the sensor frame
|
||||
Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
|
||||
Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega);
|
||||
Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
|
||||
Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
|
||||
|
||||
// Compensate for sensor-body displacement if needed: we express the quantities
|
||||
// (originally in the IMU frame) into the body frame
|
||||
|
|
@ -95,8 +94,8 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
|
|||
const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
|
||||
|
||||
// Convert angular velocity and acceleration from sensor to body frame
|
||||
j_correctedOmega = bRs * j_correctedOmega;
|
||||
j_correctedAcc = bRs * j_correctedAcc;
|
||||
correctedOmega = bRs * correctedOmega;
|
||||
correctedAcc = bRs * correctedAcc;
|
||||
|
||||
// Jacobians
|
||||
if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs;
|
||||
|
|
@ -108,21 +107,21 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
|
|||
if (!b_arm.isZero()) {
|
||||
// Subtract out the the centripetal acceleration from the measured one
|
||||
// to get linear acceleration vector in the body frame:
|
||||
const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega);
|
||||
const Matrix3 body_Omega_body = skewSymmetric(correctedOmega);
|
||||
const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
|
||||
j_correctedAcc -= body_Omega_body * b_velocity_bs;
|
||||
correctedAcc -= body_Omega_body * b_velocity_bs;
|
||||
|
||||
// Update derivative: centrifugal causes the correlation between acc and omega!!!
|
||||
if (D_correctedAcc_measuredOmega) {
|
||||
double wdp = j_correctedOmega.dot(b_arm);
|
||||
double wdp = correctedOmega.dot(b_arm);
|
||||
*D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp))
|
||||
+ j_correctedOmega * b_arm.transpose()) * bRs.matrix()
|
||||
+ 2 * b_arm * j_measuredOmega.transpose();
|
||||
+ correctedOmega * b_arm.transpose()) * bRs.matrix()
|
||||
+ 2 * b_arm * measuredOmega.transpose();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return make_pair(j_correctedAcc, j_correctedOmega);
|
||||
return make_pair(correctedAcc, correctedOmega);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -144,15 +143,24 @@ PreintegrationBase::TangentVector PreintegrationBase::UpdateEstimate(
|
|||
|
||||
if (A) {
|
||||
// First order (small angle) approximation of derivative of invH*w:
|
||||
const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body);
|
||||
// const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body);
|
||||
// NOTE(frank): Rot3::ExpmapDerivative(w_body) is also an approximation (but less accurate):
|
||||
// If we replace approximation with numerical derivative we get better accuracy:
|
||||
auto f = [w_body](const Vector3& theta) {
|
||||
return Rot3::ExpmapDerivative(theta).inverse() * w_body;
|
||||
};
|
||||
const Matrix3 invHw_H_theta = numericalDerivative11<Vector3, Vector3>(f, zeta.theta());
|
||||
|
||||
// Exact derivative of R*a with respect to theta:
|
||||
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * H;
|
||||
|
||||
A->setIdentity();
|
||||
// theta:
|
||||
A->block<3, 3>(0, 0).noalias() += invHw_H_theta * dt;
|
||||
// position:
|
||||
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22;
|
||||
A->block<3, 3>(3, 6) = I_3x3 * dt;
|
||||
// velocity:
|
||||
A->block<3, 3>(6, 0) = a_nav_H_theta * dt;
|
||||
}
|
||||
if (B) {
|
||||
|
|
@ -176,8 +184,8 @@ PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij(
|
|||
OptionalJacobian<9, 3> C) const {
|
||||
if (!p().body_P_sensor) {
|
||||
// Correct for bias in the sensor frame
|
||||
Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
|
||||
Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
|
||||
const Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
|
||||
const Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
|
||||
|
||||
// Do update in one fell swoop
|
||||
return UpdateEstimate(correctedAcc, correctedOmega, dt, deltaXij_, A, B, C);
|
||||
|
|
@ -209,8 +217,8 @@ PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij(
|
|||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegrationBase::update(const Vector3& j_measuredAcc,
|
||||
const Vector3& j_measuredOmega, double dt,
|
||||
void PreintegrationBase::update(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, double dt,
|
||||
Matrix3* D_incrR_integratedOmega, Matrix9* A,
|
||||
Matrix93* B, Matrix93* C) {
|
||||
// Save current rotation for updating Jacobians
|
||||
|
|
@ -218,44 +226,41 @@ void PreintegrationBase::update(const Vector3& j_measuredAcc,
|
|||
|
||||
// Do update
|
||||
deltaTij_ += dt;
|
||||
deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt, A, B, C);
|
||||
deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, A, B, C);
|
||||
|
||||
// Update Jacobians
|
||||
// TODO(frank): we are repeating some computation here: accessible in A ?
|
||||
// Possibly: derivatives are just -B and -C ??
|
||||
Vector3 j_correctedAcc, j_correctedOmega;
|
||||
boost::tie(j_correctedAcc, j_correctedOmega) =
|
||||
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
|
||||
// D_plus_abias = D_plus_zeta * D_zeta_abias + D_plus_a * D_a_abias
|
||||
Matrix93 D_zeta_abias, D_plus_abias;
|
||||
D_zeta_abias << Z_3x3, delPdelBiasAcc_, delVdelBiasAcc_;
|
||||
D_plus_abias = (*A) * D_zeta_abias - (*B);
|
||||
delPdelBiasAcc_ = D_plus_abias.middleRows<3>(3);
|
||||
delVdelBiasAcc_ = D_plus_abias.middleRows<3>(6);
|
||||
|
||||
double dt22 = 0.5 * dt * dt;
|
||||
const Matrix3 dRij = oldRij.matrix(); // expensive
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
|
||||
delVdelBiasAcc_ += -dRij * dt;
|
||||
|
||||
cout << "B:" << endl;
|
||||
cout << -(*B) << endl;
|
||||
cout << "update:" << endl;
|
||||
cout << - dt22 * dRij << endl;
|
||||
cout << -dRij * dt << endl;
|
||||
#ifdef USE_NEW_WAY_FOR_OMEGA
|
||||
// D_plus_wbias = D_plus_zeta * D_zeta_wbias + D_plus_w * D_w_wbias
|
||||
Matrix93 D_zeta_wbias, D_plus_wbias;
|
||||
D_zeta_wbias << delRdelBiasOmega_, delPdelBiasOmega_, delVdelBiasOmega_;
|
||||
D_plus_wbias = (*A) * D_zeta_wbias - (*C);
|
||||
delRdelBiasOmega_ = D_plus_wbias.middleRows<3>(0);
|
||||
delPdelBiasOmega_ = D_plus_wbias.middleRows<3>(3);
|
||||
delVdelBiasOmega_ = D_plus_wbias.middleRows<3>(6);
|
||||
#else
|
||||
// The old way matches the derivatives of deltaRij()
|
||||
Vector3 correctedAcc, correctedOmega;
|
||||
boost::tie(correctedAcc, correctedOmega) =
|
||||
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega);
|
||||
|
||||
Matrix3 D_acc_R;
|
||||
oldRij.rotate(j_correctedAcc, D_acc_R);
|
||||
double dt22 = 0.5 * dt * dt;
|
||||
oldRij.rotate(correctedAcc, D_acc_R);
|
||||
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
|
||||
const Vector3 integratedOmega = j_correctedOmega * dt;
|
||||
const Rot3 incrR =
|
||||
Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
|
||||
const Vector3 integratedOmega = correctedOmega * dt;
|
||||
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega);
|
||||
const Matrix3 incrRt = incrR.transpose();
|
||||
|
||||
delRdelBiasOmega_ =
|
||||
incrRt * delRdelBiasOmega_ - *D_incrR_integratedOmega * dt;
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - *D_incrR_integratedOmega * dt;
|
||||
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
|
||||
delVdelBiasOmega_ += D_acc_biasOmega * dt;
|
||||
cout << "C:" << endl;
|
||||
cout << -(*C) << endl;
|
||||
cout << "update:" << endl;
|
||||
cout << - *D_incrR_integratedOmega* dt << endl;
|
||||
cout << dt22 * D_acc_biasOmega << endl;
|
||||
cout << D_acc_biasOmega * dt << endl;
|
||||
#endif
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -226,7 +226,7 @@ public:
|
|||
/// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
|
||||
/// Ignore D_correctedOmega_measuredAcc as it is trivially zero
|
||||
std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose(
|
||||
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega,
|
||||
OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none,
|
||||
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none,
|
||||
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const;
|
||||
|
|
@ -243,14 +243,14 @@ public:
|
|||
/// Calculate the updated preintegrated measurement, does not modify
|
||||
/// It takes measured quantities in the j frame
|
||||
PreintegrationBase::TangentVector updatedDeltaXij(
|
||||
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, double dt,
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt,
|
||||
OptionalJacobian<9, 9> A = boost::none,
|
||||
OptionalJacobian<9, 3> B = boost::none,
|
||||
OptionalJacobian<9, 3> C = boost::none) const;
|
||||
|
||||
/// Update preintegrated measurements and get derivatives
|
||||
/// It takes measured quantities in the j frame
|
||||
void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
|
||||
void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
|
||||
const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* A,
|
||||
Matrix93* B, Matrix93* C);
|
||||
|
||||
|
|
|
|||
|
|
@ -34,6 +34,8 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef imuBias::ConstantBias Bias;
|
||||
|
||||
// Convenience for named keys
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::V;
|
||||
|
|
@ -43,7 +45,8 @@ static const double kGravity = 10;
|
|||
static const Vector3 kGravityAlongNavZDown(0, 0, kGravity);
|
||||
static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
|
||||
static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1);
|
||||
static const imuBias::ConstantBias kZeroBiasHat, kZeroBias;
|
||||
static const Bias kZeroBiasHat, kZeroBias;
|
||||
static const Vector3 Z_3x1 = Vector3::Zero();
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace {
|
||||
|
|
@ -51,7 +54,7 @@ namespace {
|
|||
/* ************************************************************************* */
|
||||
Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
|
||||
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias) {
|
||||
const Bias& bias) {
|
||||
return Rot3::Expmap(
|
||||
factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3));
|
||||
}
|
||||
|
|
@ -75,7 +78,7 @@ static boost::shared_ptr<PreintegrationParams> defaultParams() {
|
|||
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
|
||||
/* ************************************************************************* */
|
||||
PreintegratedImuMeasurements evaluatePreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
|
||||
const Bias& bias, const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
|
||||
PreintegratedImuMeasurements result(defaultParams(), bias);
|
||||
|
||||
|
|
@ -89,27 +92,19 @@ PreintegratedImuMeasurements evaluatePreintegratedMeasurements(
|
|||
}
|
||||
|
||||
Vector3 evaluatePreintegratedMeasurementsPosition(
|
||||
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
|
||||
const Bias& bias, const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
|
||||
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
||||
deltaTs).deltaPij();
|
||||
}
|
||||
|
||||
Vector3 evaluatePreintegratedMeasurementsVelocity(
|
||||
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
|
||||
const Bias& bias, const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
|
||||
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
||||
deltaTs).deltaVij();
|
||||
}
|
||||
|
||||
Rot3 evaluatePreintegratedMeasurementsRotation(
|
||||
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
|
||||
return Rot3(
|
||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
||||
deltaTs).deltaRij());
|
||||
}
|
||||
|
||||
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
|
||||
const double deltaT) {
|
||||
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
|
||||
|
|
@ -168,7 +163,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
|
|||
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9);
|
||||
|
||||
// Check derivatives of computeError
|
||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
|
||||
Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
|
||||
NavState x1, x2 = actual1.predict(x1, bias);
|
||||
|
||||
{
|
||||
|
|
@ -176,7 +171,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
|
|||
Matrix96 aH3;
|
||||
actual1.computeError(x1, x2, bias, aH1, aH2, aH3);
|
||||
boost::function<Vector9(const NavState&, const NavState&,
|
||||
const imuBias::ConstantBias&)> f =
|
||||
const Bias&)> f =
|
||||
boost::bind(&PreintegrationBase::computeError, actual1, _1, _2, _3,
|
||||
boost::none, boost::none, boost::none);
|
||||
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
|
||||
|
|
@ -237,7 +232,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
|
|||
// biasCorrectedDelta
|
||||
Matrix96 actualH;
|
||||
pim.biasCorrectedDelta(kZeroBias, actualH);
|
||||
Matrix expectedH = numericalDerivative11<Vector9, imuBias::ConstantBias>(
|
||||
Matrix expectedH = numericalDerivative11<Vector9, Bias>(
|
||||
boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
|
||||
boost::none), kZeroBias);
|
||||
EXPECT(assert_equal(expectedH, actualH));
|
||||
|
|
@ -249,7 +244,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
|
|||
boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none,
|
||||
boost::none), state1);
|
||||
EXPECT(assert_equal(eH1, aH1));
|
||||
Matrix eH2 = numericalDerivative11<NavState, imuBias::ConstantBias>(
|
||||
Matrix eH2 = numericalDerivative11<NavState, Bias>(
|
||||
boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none,
|
||||
boost::none), kZeroBias);
|
||||
EXPECT(assert_equal(eH2, aH2));
|
||||
|
|
@ -328,7 +323,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
|
|||
using common::x1;
|
||||
using common::v1;
|
||||
using common::v2;
|
||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
|
||||
Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
|
||||
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
|
||||
Point3(5.5, 1.0, -50.0));
|
||||
|
||||
|
|
@ -342,14 +337,14 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
|
|||
auto p = defaultParams();
|
||||
p->omegaCoriolis = kNonZeroOmegaCoriolis;
|
||||
|
||||
imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1));
|
||||
Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1));
|
||||
PreintegratedImuMeasurements pim(p, biasHat);
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Make sure of biasCorrectedDelta
|
||||
Matrix96 actualH;
|
||||
pim.biasCorrectedDelta(bias, actualH);
|
||||
Matrix expectedH = numericalDerivative11<Vector9, imuBias::ConstantBias>(
|
||||
Matrix expectedH = numericalDerivative11<Vector9, Bias>(
|
||||
boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
|
||||
boost::none), bias);
|
||||
EXPECT(assert_equal(expectedH, actualH));
|
||||
|
|
@ -374,7 +369,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
|
|||
using common::x1;
|
||||
using common::v1;
|
||||
using common::v2;
|
||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
|
||||
Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
|
||||
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
|
||||
Point3(5.5, 1.0, -50.0));
|
||||
|
||||
|
|
@ -390,7 +385,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
|
|||
p->use2ndOrderCoriolis = true;
|
||||
|
||||
PreintegratedImuMeasurements pim(p,
|
||||
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)));
|
||||
Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)));
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
|
|
@ -502,40 +497,40 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
|
|||
// Actual pre-integrated values
|
||||
PreintegratedImuMeasurements preintegrated =
|
||||
evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas,
|
||||
deltaTs);
|
||||
deltaTs);
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelPdelBias = numericalDerivative11<Vector,
|
||||
imuBias::ConstantBias>(
|
||||
// Check derivative of rotation
|
||||
boost::function<Rot3(const Vector3&, const Vector3&)> theta = [=](
|
||||
const Vector3& a, const Vector3& w) {
|
||||
return evaluatePreintegratedMeasurements(
|
||||
Bias(a, w), measuredAccs, measuredOmegas, deltaTs).deltaRij();
|
||||
};
|
||||
EXPECT(
|
||||
assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3)));
|
||||
EXPECT(assert_equal(numericalDerivative22(theta, Z_3x1, Z_3x1),
|
||||
preintegrated.delRdelBiasOmega(), 1e-7));
|
||||
|
||||
// Check derivative of translation
|
||||
Matrix expectedDelPdelBias = numericalDerivative11<Vector, Bias>(
|
||||
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs,
|
||||
measuredOmegas, deltaTs), kZeroBias);
|
||||
measuredOmegas, deltaTs),
|
||||
kZeroBias);
|
||||
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
|
||||
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
|
||||
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
|
||||
EXPECT(assert_equal(expectedDelPdelBiasOmega,
|
||||
preintegrated.delPdelBiasOmega(), 1e-8));
|
||||
|
||||
Matrix expectedDelVdelBias = numericalDerivative11<Vector,
|
||||
imuBias::ConstantBias>(
|
||||
// Check derivative of velocity
|
||||
Matrix expectedDelVdelBias = numericalDerivative11<Vector, Bias>(
|
||||
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs,
|
||||
measuredOmegas, deltaTs), kZeroBias);
|
||||
measuredOmegas, deltaTs),
|
||||
kZeroBias);
|
||||
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
|
||||
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
|
||||
|
||||
Matrix expectedDelRdelBias =
|
||||
numericalDerivative11<Rot3, imuBias::ConstantBias>(
|
||||
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
|
||||
measuredAccs, measuredOmegas, deltaTs), kZeroBias);
|
||||
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
|
||||
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
|
||||
EXPECT(
|
||||
assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega(),1e-8));
|
||||
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
|
||||
EXPECT(
|
||||
assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega(),1e-8));
|
||||
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
|
||||
EXPECT(
|
||||
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),1e-7));
|
||||
EXPECT(assert_equal(expectedDelVdelBiasOmega,
|
||||
preintegrated.delVdelBiasOmega(), 1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -558,7 +553,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
|||
Point3(0.1, 0.05, 0.01));
|
||||
p->omegaCoriolis = kNonZeroOmegaCoriolis;
|
||||
|
||||
imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
|
||||
Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
|
||||
|
||||
const double T = 3.0; // seconds
|
||||
ScenarioRunner runner(&scenario, p, T / 10);
|
||||
|
|
@ -608,7 +603,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
|||
// measuredOmega, 1e-6);
|
||||
// EXPECT(assert_equal(expectedG2, G2, 1e-5));
|
||||
|
||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||
Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||
|
||||
// integrate at least twice to get position information
|
||||
// otherwise factor cov noise from preint_cov is not positive definite
|
||||
|
|
@ -636,7 +631,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, PredictPositionAndVelocity) {
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||
Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega;
|
||||
|
|
@ -646,7 +641,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
|
|||
double deltaT = 0.001;
|
||||
|
||||
PreintegratedImuMeasurements pim(defaultParams(),
|
||||
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
|
||||
Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
|
||||
|
||||
for (int i = 0; i < 1000; ++i)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
|
@ -664,7 +659,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, PredictRotation) {
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||
Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega;
|
||||
|
|
@ -674,7 +669,7 @@ TEST(ImuFactor, PredictRotation) {
|
|||
double deltaT = 0.001;
|
||||
|
||||
PreintegratedImuMeasurements pim(defaultParams(),
|
||||
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
|
||||
Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
|
||||
|
||||
for (int i = 0; i < 1000; ++i)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
|
@ -684,7 +679,7 @@ TEST(ImuFactor, PredictRotation) {
|
|||
|
||||
// Predict
|
||||
NavState actual = pim.predict(NavState(), bias);
|
||||
NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(), Vector3::Zero());
|
||||
NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(), Z_3x1);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
|
|
@ -706,7 +701,7 @@ TEST(ImuFactor, PredictArbitrary) {
|
|||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
//////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
|
||||
Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega = runner.actualAngularVelocity(0);
|
||||
|
|
@ -715,7 +710,7 @@ TEST(ImuFactor, PredictArbitrary) {
|
|||
auto p = defaultParams();
|
||||
p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise
|
||||
PreintegratedImuMeasurements pim(p, biasHat);
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
// EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega,
|
||||
// Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000));
|
||||
|
||||
|
|
@ -742,7 +737,7 @@ TEST(ImuFactor, PredictArbitrary) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, bodyPSensorNoBias) {
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
|
||||
Bias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
|
||||
|
||||
// Rotate sensor (z-down) to body (same as navigation) i.e. z-up
|
||||
auto p = defaultParams();
|
||||
|
|
@ -784,7 +779,7 @@ TEST(ImuFactor, bodyPSensorNoBias) {
|
|||
|
||||
TEST(ImuFactor, bodyPSensorWithBias) {
|
||||
using noiseModel::Diagonal;
|
||||
typedef imuBias::ConstantBias Bias;
|
||||
typedef Bias Bias;
|
||||
|
||||
int numFactors = 80;
|
||||
Vector6 noiseBetweenBiasSigma;
|
||||
|
|
@ -890,7 +885,7 @@ TEST(ImuFactor, serialization) {
|
|||
p->gyroscopeCovariance = 1e-8 * I_3x3;
|
||||
p->integrationCovariance = 1e-9 * I_3x3;
|
||||
double deltaT = 0.005;
|
||||
imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
|
||||
Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
|
||||
|
||||
PreintegratedImuMeasurements pim(p, priorBias);
|
||||
|
||||
|
|
|
|||
|
|
@ -69,7 +69,7 @@ TEST(PreintegrationBase, UpdateEstimate2) {
|
|||
pim.UpdateEstimate(acc, omega, kDt, zeta, aH1, aH2, aH3);
|
||||
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
|
||||
EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3));
|
||||
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-7));
|
||||
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8));
|
||||
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue