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

Made ImuFactor Predict function static, which calls PreintegrationBase non-static member. Fixed to upper case Predict.

release/4.3a0
krunalchande 2015-03-08 15:23:20 -04:00
parent 9a469ad25f
commit a814534c92
5 changed files with 64 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/navigation/CombinedImuFactor.h
View File

@ -213,12 +213,12 @@ public:
boost::optional<Matrix&> H6 = boost::none) const; boost::optional<Matrix&> H6 = boost::none) const;
/** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
static PoseVelocityBias predict(const CombinedPreintegratedMeasurements& PIM, const Pose3& pose_i, const Vector3& vel_i, static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& gravity, const imuBias::ConstantBias& bias_i, const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none, boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) { boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
return PreintegrationBase::predict(PIM, pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out); return PIM.Predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out);
} }

6
gtsam/navigation/ImuFactor.h
View File

@ -195,12 +195,12 @@ public:
boost::optional<Matrix&> H5 = boost::none) const; boost::optional<Matrix&> H5 = boost::none) const;
/** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
static PoseVelocityBias predict(const PreintegratedMeasurements& PIM, const Pose3& pose_i, const Vector3& vel_i, static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& gravity, const imuBias::ConstantBias& bias_i, const PreintegratedMeasurements& PIM, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none, boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) { boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
return PreintegrationBase::predict(PIM, pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out); return PIM.Predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out);
} }
private: private:

32
gtsam/navigation/PreintegrationBase.h
View File

@ -199,48 +199,48 @@ public:
/// Predict state at time j /// Predict state at time j
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
static PoseVelocityBias predict(const PreintegrationBase& PIB, const Pose3& pose_i, const Vector3& vel_i, PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& gravity, const imuBias::ConstantBias& bias_i, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none, boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) { boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const {
const Vector3 biasAccIncr = bias_i.accelerometer() - PIB.biasHat().accelerometer(); const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - PIB.biasHat().gyroscope(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
const Rot3& Rot_i = pose_i.rotation(); const Rot3& Rot_i = pose_i.rotation();
const Vector3& pos_i = pose_i.translation().vector(); const Vector3& pos_i = pose_i.translation().vector();
// Predict state at time j // Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
Vector3 deltaPij_biascorrected = PIB.deltaPij() + PIB.delPdelBiasAcc() * biasAccIncr + PIB.delPdelBiasOmega() * biasOmegaIncr; Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr;
if(deltaPij_biascorrected_out)// if desired, store this if(deltaPij_biascorrected_out)// if desired, store this
*deltaPij_biascorrected_out = deltaPij_biascorrected; *deltaPij_biascorrected_out = deltaPij_biascorrected;
Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected
+ vel_i * PIB.deltaTij() + vel_i * deltaTij()
- omegaCoriolis.cross(vel_i) * PIB.deltaTij()*PIB.deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * PIB.deltaTij()*PIB.deltaTij(); + 0.5 * gravity * deltaTij()*deltaTij();
Vector3 deltaVij_biascorrected = PIB.deltaVij() + PIB.delVdelBiasAcc() * biasAccIncr + PIB.delVdelBiasOmega() * biasOmegaIncr; Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr;
if(deltaVij_biascorrected_out)// if desired, store this if(deltaVij_biascorrected_out)// if desired, store this
*deltaVij_biascorrected_out = deltaVij_biascorrected; *deltaVij_biascorrected_out = deltaVij_biascorrected;
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected
- 2 * omegaCoriolis.cross(vel_i) * PIB.deltaTij() // Coriolis term - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term
+ gravity * PIB.deltaTij()); + gravity * deltaTij());
if(use2ndOrderCoriolis){ if(use2ndOrderCoriolis){
pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * PIB.deltaTij()*PIB.deltaTij(); // 2nd order coriolis term for position pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position
vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * PIB.deltaTij(); // 2nd order term for velocity vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity
} }
const Rot3 deltaRij_biascorrected = PIB.biascorrectedDeltaRij(biasOmegaIncr); const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
// deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
Vector3 correctedOmega = biascorrectedOmega - Vector3 correctedOmega = biascorrectedOmega -
Rot_i.inverse().matrix() * omegaCoriolis * PIB.deltaTij(); // Coriolis term Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
const Rot3 correctedDeltaRij = const Rot3 correctedDeltaRij =
Rot3::Expmap( correctedOmega ); Rot3::Expmap( correctedOmega );
const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij ); const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij );
@ -273,7 +273,7 @@ public:
// Evaluate residual error, according to [3] // Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
Vector3 deltaPij_biascorrected, deltaVij_biascorrected; Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
PoseVelocityBias predictedState_j = PreintegrationBase::predict(*this, pose_i, vel_i, bias_i, gravity, PoseVelocityBias predictedState_j = Predict(pose_i, vel_i, bias_i, gravity,
omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected); omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected);
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance

4
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -294,7 +294,7 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){
// Predict // Predict
Pose3 x1; Pose3 x1;
Vector3 v1(0, 0.0, 0.0); Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocityBias = PreintegrationBase::predict(Combined_pre_int_data, x1, v1, bias, gravity, omegaCoriolis); PoseVelocityBias poseVelocityBias = Combined_pre_int_data.Predict(x1, v1, bias, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; expectedVelocity<<0,1,0; Vector3 expectedVelocity; expectedVelocity<<0,1,0;
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
@ -322,7 +322,7 @@ TEST(CombinedImuFactor, PredictRotation) {
// Predict // Predict
Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0));
Vector3 v(0,0,0); Vector3 v(0,0,0);
PoseVelocityBias poseVelocityBias = CombinedImuFactor::predict(Combinedfactor.preintegratedMeasurements(), x,v,bias, gravity, omegaCoriolis); PoseVelocityBias poseVelocityBias = CombinedImuFactor::Predict(x,v,bias, Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0));
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol));
} }

80
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -131,8 +131,8 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
measuredAccs, measuredOmegas, deltaTs).deltaRij()); measuredAccs, measuredOmegas, deltaTs).deltaRij());
} }
Rot3 evaluateRotation(const Vector3 b_omegaMeas_nb, const Vector3 biasOmega, const double deltaT){ Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){
return Rot3::Expmap((b_omegaMeas_nb - biasOmega) * deltaT); return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
} }
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
@ -148,8 +148,8 @@ TEST( ImuFactor, PreintegratedMeasurements )
imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases
// Measurements // Measurements
Vector3 b_accMeas(0.1, 0.0, 0.0); Vector3 measuredAcc(0.1, 0.0, 0.0);
Vector3 b_omegaMeas_nb(M_PI/100.0, 0.0, 0.0); Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
double deltaT = 0.5; double deltaT = 0.5;
// Expected preintegrated values // Expected preintegrated values
@ -161,7 +161,7 @@ TEST( ImuFactor, PreintegratedMeasurements )
bool use2ndOrderIntegration = true; bool use2ndOrderIntegration = true;
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
actual1.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6));
@ -170,13 +170,13 @@ TEST( ImuFactor, PreintegratedMeasurements )
// Integrate again // Integrate again
Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0; Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0;
Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * b_accMeas * 0.5; Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5;
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0); Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0);
double expectedDeltaT2(1); double expectedDeltaT2(1);
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements actual2 = actual1; ImuFactor::PreintegratedMeasurements actual2 = actual1;
actual2.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6));
@ -197,12 +197,12 @@ TEST( ImuFactor, ErrorAndJacobians )
// Measurements // Measurements
Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 n_gravity; n_gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << M_PI/100, 0, 0; Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0;
Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).vector(); Vector3 measuredAcc = x1.rotation().unrotate(-Point3(n_gravity)).vector();
double deltaT = 1.0; double deltaT = 1.0;
bool use2ndOrderIntegration = true; bool use2ndOrderIntegration = true;
ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis);
@ -264,13 +264,13 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases )
// Measurements // Measurements
Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 n_gravity; n_gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, M_PI/10.0+0.3; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).vector() + Vector3(0.2,0.0,0.0); Vector3 measuredAcc = x1.rotation().unrotate(-Point3(n_gravity)).vector() + Vector3(0.2,0.0,0.0);
double deltaT = 1.0; double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis);
@ -327,13 +327,13 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis )
// Measurements // Measurements
Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 n_gravity; n_gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, M_PI/10.0+0.3; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).vector() + Vector3(0.2,0.0,0.0); Vector3 measuredAcc = x1.rotation().unrotate(-Point3(n_gravity)).vector() + Vector3(0.2,0.0,0.0);
double deltaT = 1.0; double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
Pose3 bodyPsensor = Pose3(); Pose3 bodyPsensor = Pose3();
@ -387,14 +387,14 @@ TEST( ImuFactor, PartialDerivative_wrt_Bias )
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
// Measurements // Measurements
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0.1, 0, 0; Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
double deltaT = 0.5; double deltaT = 0.5;
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind( Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind(
&evaluateRotation, b_omegaMeas_nb, _1, deltaT), Vector3(biasOmega)); &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
const Matrix3 Jr = Rot3::ExpmapDerivative((b_omegaMeas_nb - biasOmega) * deltaT); const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
@ -428,20 +428,20 @@ TEST( ImuFactor, fistOrderExponential )
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
// Measurements // Measurements
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0.1, 0, 0; Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
double deltaT = 1.0; double deltaT = 1.0;
// change w.r.t. linearization point // change w.r.t. linearization point
double alpha = 0.0; double alpha = 0.0;
Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha;
const Matrix3 Jr = Rot3::ExpmapDerivative((b_omegaMeas_nb - biasOmega) * deltaT); const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
const Matrix expectedRot = Rot3::Expmap((b_omegaMeas_nb - biasOmega - deltabiasOmega) * deltaT).matrix(); const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
const Matrix3 hatRot = Rot3::Expmap((b_omegaMeas_nb - biasOmega) * deltaT).matrix(); const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 actualRot = const Matrix3 actualRot =
hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
//hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
@ -763,11 +763,11 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt )
// ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); // ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
// //
// // Pre-integrate Measurements // // Pre-integrate Measurements
// Vector3 b_accMeas(0.1, 0.0, 0.0); // Vector3 measuredAcc(0.1, 0.0, 0.0);
// Vector3 b_omegaMeas_nb(M_PI/100.0, 0.0, 0.0); // Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
// double deltaT = 0.5; // double deltaT = 0.5;
// for(size_t i = 0; i < 50; ++i) { // for(size_t i = 0; i < 50; ++i) {
// pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); // pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// } // }
// //
// // Create factor // // Create factor
@ -813,8 +813,8 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
// Measurements // Measurements
Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 n_gravity; n_gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, M_PI/10.0+0.3; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).vector() + Vector3(0.2,0.0,0.0); Vector3 measuredAcc = x1.rotation().unrotate(-Point3(n_gravity)).vector() + Vector3(0.2,0.0,0.0);
double deltaT = 1.0; double deltaT = 1.0;
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0)); const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0));
@ -822,7 +822,7 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis);
@ -865,8 +865,8 @@ TEST(ImuFactor, PredictPositionAndVelocity){
// Measurements // Measurements
Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 n_gravity; n_gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, 0;//M_PI/10.0+0.3; Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3;
Vector3 b_accMeas; b_accMeas << 0,1,-9.81; Vector3 measuredAcc; measuredAcc << 0,1,-9.81;
double deltaT = 0.001; double deltaT = 0.001;
Matrix I6x6(6,6); Matrix I6x6(6,6);
@ -875,7 +875,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis);
@ -883,7 +883,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){
// Predict // Predict
Pose3 x1; Pose3 x1;
Vector3 v1(0, 0.0, 0.0); Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocity = PreintegrationBase::predict(pre_int_data, x1, v1, bias, n_gravity, omegaCoriolis); PoseVelocityBias poseVelocity = pre_int_data.Predict(x1, v1, bias, n_gravity, omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; expectedVelocity<<0,1,0; Vector3 expectedVelocity; expectedVelocity<<0,1,0;
EXPECT(assert_equal(expectedPose, poseVelocity.pose)); EXPECT(assert_equal(expectedPose, poseVelocity.pose));
@ -897,8 +897,8 @@ TEST(ImuFactor, PredictRotation) {
// Measurements // Measurements
Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 n_gravity; n_gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, M_PI/10;//M_PI/10.0+0.3; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3;
Vector3 b_accMeas; b_accMeas << 0,0,-9.81; Vector3 measuredAcc; measuredAcc << 0,0,-9.81;
double deltaT = 0.001; double deltaT = 0.001;
Matrix I6x6(6,6); Matrix I6x6(6,6);
@ -907,7 +907,7 @@ TEST(ImuFactor, PredictRotation) {
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis);
@ -915,7 +915,7 @@ TEST(ImuFactor, PredictRotation) {
// Predict // Predict
Pose3 x1; Pose3 x1;
Vector3 v1(0, 0.0, 0.0); Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocity = ImuFactor::predict(factor.preintegratedMeasurements(), x1, v1, bias, n_gravity, omegaCoriolis); PoseVelocityBias poseVelocity = ImuFactor::Predict(x1, v1, bias, factor.preintegratedMeasurements(), n_gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0)); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0));
Vector3 expectedVelocity; expectedVelocity<<0,0,0; Vector3 expectedVelocity; expectedVelocity<<0,0,0;
EXPECT(assert_equal(expectedPose, poseVelocity.pose)); EXPECT(assert_equal(expectedPose, poseVelocity.pose));
@ -933,8 +933,8 @@ TEST(ImuFactor, CheckBiasCorrection) {
int numSamplesPreint = 1; int numSamplesPreint = 1;
double g = 9.81; double g = 9.81;
// Measurements. Body frame and nav frame are both z-up // Measurements. Body frame and nav frame are both z-up
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0.3, 0.0; Vector3 measuredOmega; measuredOmega << 0, 0.3, 0.0;
Vector3 b_accMeas; b_accMeas << 0, 0, g; Vector3 measuredAcc; measuredAcc << 0, 0, g;
Vector3 n_gravity; n_gravity << 0, 0, -g; Vector3 n_gravity; n_gravity << 0, 0, -g;
// Set up noise and other test params // Set up noise and other test params
@ -972,7 +972,7 @@ TEST(ImuFactor, CheckBiasCorrection) {
// Preintegrate measurements // Preintegrate measurements
ImuFactor::PreintegratedMeasurements pre_int_data = ImuFactor::PreintegratedMeasurements(imuBias::ConstantBias(Vector3(0, 0.0, 0.0), ImuFactor::PreintegratedMeasurements pre_int_data = ImuFactor::PreintegratedMeasurements(imuBias::ConstantBias(Vector3(0, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)), accCov, gyroCov, integrationCov, true); Vector3(0.0, 0.0, 0.0)), accCov, gyroCov, integrationCov, true);
for (int j = 0; j< numSamplesPreint; ++j) pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); for (int j = 0; j< numSamplesPreint; ++j) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create and add factor // Create and add factor
ImuFactor factor(X(i-1), V(i-1), X(i), V(i), B(i-1), pre_int_data, n_gravity, omegaCoriolis); ImuFactor factor(X(i-1), V(i-1), X(i), V(i), B(i-1), pre_int_data, n_gravity, omegaCoriolis);