in the process of adding tests for the combined imu factor (not there yet)
Luca
parent
b126d98609
commit
64dfde3ae6
|
|
@ -71,7 +71,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){
|
|||
//------------------------------------------------------------------------------
|
||||
void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega,
|
||||
double deltaT, boost::optional<const Pose3&> body_P_sensor) {
|
||||
double deltaT, boost::optional<const Pose3&> body_P_sensor,
|
||||
boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
|
||||
|
||||
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
|
||||
// (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
|
||||
|
|
@ -129,7 +130,6 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
|
|||
|
||||
// first order uncertainty propagation
|
||||
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose()
|
||||
|
||||
Matrix G_measCov_Gt = Matrix::Zero(15,15);
|
||||
// BLOCK DIAGONAL TERMS
|
||||
G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0);
|
||||
|
|
@ -152,6 +152,22 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
|
|||
G_measCov_Gt.block<3,3>(6,3) = block23.transpose();
|
||||
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
|
||||
|
||||
// F_test and G_test are used for testing purposes and are not needed by the factor
|
||||
if(F_test){
|
||||
F_test->resize(15,15);
|
||||
(*F_test) << F;
|
||||
}
|
||||
if(G_test){
|
||||
G_test->resize(15,21);
|
||||
// This is for testing & documentation
|
||||
///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
|
||||
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3,
|
||||
Z_3x3, H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3,
|
||||
Z_3x3, Z_3x3, H_angles_angles, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega,
|
||||
Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3,
|
||||
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3;
|
||||
}
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -126,7 +126,8 @@ public:
|
|||
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
|
||||
*/
|
||||
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none);
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none,
|
||||
boost::optional<Matrix&> F_test = boost::none, boost::optional<Matrix&> G_test = boost::none);
|
||||
|
||||
/// methods to access class variables
|
||||
Matrix measurementCovariance() const {return measurementCovariance_;}
|
||||
|
|
|
|||
|
|
@ -39,19 +39,44 @@ using symbol_shorthand::X;
|
|||
using symbol_shorthand::V;
|
||||
using symbol_shorthand::B;
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace {
|
||||
/* ************************************************************************* */
|
||||
// Auxiliary functions to test Jacobians F and G used for
|
||||
// covariance propagation during preintegration
|
||||
/* ************************************************************************* */
|
||||
Vector updatePreintegratedMeasurementsTest(
|
||||
const Vector3 deltaPij_old, const Vector3& deltaVij_old,
|
||||
const Vector3& logDeltaRij_old, const imuBias::ConstantBias& bias_old,
|
||||
const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT,
|
||||
const bool use2ndOrderIntegration_) {
|
||||
|
||||
ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
|
||||
Rot3 deltaRij_old = Rot3::Expmap(logDeltaRij_old);
|
||||
Matrix3 dRij = deltaRij_old.matrix();
|
||||
Vector3 temp = dRij * correctedAcc * deltaT;
|
||||
Vector3 deltaPij_new;
|
||||
if(!use2ndOrderIntegration_){
|
||||
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
|
||||
}else{
|
||||
deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
|
||||
}
|
||||
Vector3 deltaVij_new = deltaVij_old + temp;
|
||||
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT);
|
||||
Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new);
|
||||
imuBias::ConstantBias bias_new(bias_old);
|
||||
Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector();
|
||||
return result;
|
||||
}
|
||||
|
||||
// Auxiliary functions to test preintegrated Jacobians
|
||||
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
|
||||
/* ************************************************************************* */
|
||||
CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias,
|
||||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
|
||||
)
|
||||
{
|
||||
ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
|
||||
Matrix3::Identity(), Matrix3::Identity());
|
||||
const list<double>& deltaTs){
|
||||
CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(),
|
||||
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false);
|
||||
|
||||
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
|
||||
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
|
||||
|
|
@ -59,7 +84,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
|
|||
for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
|
||||
result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -67,20 +91,16 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
|
|||
const imuBias::ConstantBias& bias,
|
||||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
const list<double>& deltaTs){
|
||||
return evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij();
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaPij();
|
||||
}
|
||||
|
||||
Vector3 evaluatePreintegratedMeasurementsVelocity(
|
||||
const imuBias::ConstantBias& bias,
|
||||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
const list<double>& deltaTs){
|
||||
return evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaVij();
|
||||
}
|
||||
|
|
@ -89,9 +109,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
|
|||
const imuBias::ConstantBias& bias,
|
||||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
const list<double>& deltaTs){
|
||||
return Rot3(evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaRij());
|
||||
}
|
||||
|
|
@ -101,7 +119,6 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
|
|||
/* ************************************************************************* */
|
||||
TEST( CombinedImuFactor, PreintegratedMeasurements )
|
||||
{
|
||||
//cout << "++++++++++++++++++++++++++++++ PreintegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl;
|
||||
// Linearization point
|
||||
imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases
|
||||
|
||||
|
|
@ -120,28 +137,17 @@ TEST( CombinedImuFactor, PreintegratedMeasurements )
|
|||
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
||||
Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6));
|
||||
|
||||
// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
|
||||
// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
|
||||
// const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
|
||||
// const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
|
||||
// const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
|
||||
// const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
|
||||
// const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
|
||||
|
||||
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol));
|
||||
// EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol));
|
||||
// EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol));
|
||||
// DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol);
|
||||
EXPECT(assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), tol));
|
||||
EXPECT(assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol));
|
||||
DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( CombinedImuFactor, ErrorWithBiases )
|
||||
{
|
||||
//cout << "++++++++++++++++++++++++++++++ ErrorWithBiases +++++++++++++++++++++++++++++++++++++++ " << endl;
|
||||
|
||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||
imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
|
||||
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
|
||||
|
|
@ -157,50 +163,37 @@ TEST( CombinedImuFactor, ErrorWithBiases )
|
|||
double deltaT = 1.0;
|
||||
double tol = 1e-6;
|
||||
|
||||
// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
|
||||
// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
|
||||
// const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
|
||||
// const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
|
||||
// const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
|
||||
// const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
|
||||
// const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
|
||||
|
||||
Matrix I6x6(6,6);
|
||||
I6x6 = Matrix::Identity(6,6);
|
||||
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
|
||||
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
|
||||
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
|
||||
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
|
||||
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 );
|
||||
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
|
||||
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
|
||||
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 );
|
||||
|
||||
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
|
||||
// Create factor
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
|
||||
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
|
||||
|
||||
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
|
||||
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
|
||||
Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);
|
||||
|
||||
Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2);
|
||||
|
||||
Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);
|
||||
EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
|
||||
|
||||
Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2);
|
||||
// Expected Jacobians
|
||||
Matrix H1e, H2e, H3e, H4e, H5e;
|
||||
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
|
||||
|
||||
|
||||
EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
|
||||
|
||||
// Expected Jacobians
|
||||
Matrix H1e, H2e, H3e, H4e, H5e;
|
||||
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
|
||||
|
||||
|
||||
// Actual Jacobians
|
||||
// Actual Jacobians
|
||||
Matrix H1a, H2a, H3a, H4a, H5a, H6a;
|
||||
(void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a);
|
||||
|
||||
|
|
@ -214,7 +207,6 @@ TEST( CombinedImuFactor, ErrorWithBiases )
|
|||
/* ************************************************************************* */
|
||||
TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
|
||||
{
|
||||
//cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl;
|
||||
// Linearization point
|
||||
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
|
||||
|
||||
|
|
@ -237,22 +229,22 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
|
|||
}
|
||||
|
||||
// Actual preintegrated values
|
||||
ImuFactor::PreintegratedMeasurements preintegrated =
|
||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
|
||||
CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
|
||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs);
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
|
||||
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
|
||||
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias);
|
||||
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
|
||||
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
|
||||
|
||||
Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
|
||||
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
|
||||
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias);
|
||||
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
|
||||
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
|
||||
|
||||
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
|
||||
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
|
||||
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias);
|
||||
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
|
||||
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
|
||||
|
||||
|
|
@ -265,6 +257,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
|
|||
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(CombinedImuFactor, PredictPositionAndVelocity){
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||
|
||||
|
|
@ -283,22 +276,21 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){
|
|||
|
||||
for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
|
||||
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
|
||||
|
||||
// Predict
|
||||
Pose3 x1;
|
||||
Vector3 v1(0, 0.0, 0.0);
|
||||
PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
|
||||
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
|
||||
Vector3 expectedVelocity; expectedVelocity<<0,1,0;
|
||||
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
|
||||
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity)));
|
||||
|
||||
// Create factor
|
||||
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
|
||||
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
|
||||
|
||||
// Predict
|
||||
Pose3 x1;
|
||||
Vector3 v1(0, 0.0, 0.0);
|
||||
PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
|
||||
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
|
||||
Vector3 expectedVelocity; expectedVelocity<<0,1,0;
|
||||
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
|
||||
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(CombinedImuFactor, PredictRotation) {
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||
Matrix I6x6(6,6);
|
||||
|
|
@ -324,9 +316,88 @@ TEST(CombinedImuFactor, PredictRotation) {
|
|||
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol));
|
||||
}
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
/* ************************************************************************* */
|
||||
TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
|
||||
{
|
||||
// Linearization point
|
||||
imuBias::ConstantBias bias = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases
|
||||
Pose3 body_P_sensor = Pose3();
|
||||
|
||||
// Measurements
|
||||
list<Vector3> measuredAccs, measuredOmegas;
|
||||
list<double> deltaTs;
|
||||
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
|
||||
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
|
||||
deltaTs.push_back(0.01);
|
||||
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
|
||||
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
|
||||
deltaTs.push_back(0.01);
|
||||
for(int i=1;i<100;i++)
|
||||
{
|
||||
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
|
||||
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
|
||||
deltaTs.push_back(0.01);
|
||||
}
|
||||
// Actual preintegrated values
|
||||
CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
|
||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs);
|
||||
|
||||
// so far we only created a nontrivial linearization point for the preintegrated measurements
|
||||
// Now we add a new measurement and ask for Jacobians
|
||||
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
|
||||
const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0);
|
||||
const double newDeltaT = 0.01;
|
||||
const Vector3 logDeltaRij_old = preintegrated.thetaRij(); // before adding new measurement
|
||||
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
|
||||
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
|
||||
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
|
||||
//
|
||||
// Matrix Factual, Gactual;
|
||||
// preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
|
||||
// body_P_sensor, Factual, Gactual);
|
||||
//
|
||||
// bool use2ndOrderIntegration = false;
|
||||
//
|
||||
// // Compute expected F wrt positions
|
||||
// Matrix df_dpos =
|
||||
// numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
|
||||
// _1, deltaVij_old, logDeltaRij_old,
|
||||
// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
|
||||
// // Compute expected F wrt velocities
|
||||
// Matrix df_dvel =
|
||||
// numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
|
||||
// deltaPij_old, _1, logDeltaRij_old,
|
||||
// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
|
||||
// // Compute expected F wrt angles
|
||||
// Matrix df_dangle =
|
||||
// numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
|
||||
// deltaPij_old, deltaVij_old, _1,
|
||||
// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old);
|
||||
// Matrix Fexpected(9,9);
|
||||
//
|
||||
// Fexpected << df_dpos, df_dvel, df_dangle;
|
||||
// EXPECT(assert_equal(Fexpected, Factual));
|
||||
//
|
||||
// // Compute expected G wrt integration noise
|
||||
// Matrix df_dintNoise(9,3);
|
||||
// df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3;
|
||||
//
|
||||
// // Compute expected F wrt acc noise
|
||||
// Matrix df_daccNoise =
|
||||
// numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
|
||||
// deltaPij_old, deltaVij_old, logDeltaRij_old,
|
||||
// _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
|
||||
// // Compute expected F wrt gyro noise
|
||||
// Matrix df_domegaNoise =
|
||||
// numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
|
||||
// deltaPij_old, deltaVij_old, logDeltaRij_old,
|
||||
// newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
|
||||
// Matrix Gexpected(9,9);
|
||||
//
|
||||
// Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise;
|
||||
// EXPECT(assert_equal(Gexpected, Gactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue