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

truth revealing unit test :-) re-established good functioning of IMU factor (TODO: fix CombinedImuFactor F & G)

release/4.3a0
Luca 2014-12-07 13:14:45 -05:00
parent 792d2656d0
commit 6d571ca6b9
4 changed files with 107 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/navigation/ImuFactor.cpp
View File

@ -67,7 +67,8 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){
//------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor) {
boost::optional<const Pose3&> body_P_sensor,
boost::optional<Matrix&> Fout, boost::optional<Matrix&> Gout) {
// NOTE: order is important here because each update uses old values (i.e., we have to update
// jacobians and covariances before updating preintegrated measurements).
@ -87,6 +88,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
/* ----------------------------------------------------------------------------------------------------------------------- */
const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3)
const Matrix3 R_i = deltaRij();
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
// Update preintegrated measurements. TODO Frank moved from end of this function !!!
@ -102,7 +104,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
Matrix H_vel_pos = Z_3x3;
Matrix H_vel_vel = I_3x3;
Matrix H_vel_angles = - deltaRij() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
Matrix H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
@ -124,15 +126,20 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
// This in only kept for documentation.
//
// Matrix G(9,9);
// G << I_3x3 * deltaT, Z_3x3, Z_3x3,
// Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
// Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
//
// preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
// Fout and Gout are used for testing purposes and are not needed by the factor
if(Fout){
Fout->resize(9,9);
(*Fout) << F;
}
if(Gout){
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
// This in only kept for testing.
Gout->resize(9,9);
(*Gout) << I_3x3 * deltaT, Z_3x3, Z_3x3,
Z_3x3, R_i * deltaT, Z_3x3,
Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
//preintMeasCov = F * preintMeasCov * F.transpose() + Gout * (1/deltaT) * measurementCovariance * Gout.transpose();
}
}
//------------------------------------------------------------------------------

4
gtsam/navigation/ImuFactor.h
View File

@ -111,9 +111,11 @@ public:
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param deltaT Time interval between two consecutive IMU measurements
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
* @param Fout, Gout Jacobians used internally (only needed for testing)
*/
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&> Fout = boost::none, boost::optional<Matrix&> Gout = boost::none);
/// methods to access class variables
Matrix measurementCovariance() const {return measurementCovariance_;}

22
gtsam/navigation/PreintegrationBase.h
View File

@ -343,28 +343,6 @@ public:
return r;
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
const Vector3& delta_angles, const Vector& delta_vel_in_t0){
// Note: all delta terms refer to an IMU\sensor system at t0
Vector body_t_a_body = msr_acc_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
}
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles){
// Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object
Vector body_t_omega_body= msr_gyro_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
return Rot3::Logmap(R_t_to_t0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
private:
/** Serialization function */
friend class boost::serialization::access;

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

@ -49,6 +49,23 @@ Rot3 evaluateRotationError(const ImuFactor& factor,
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
}
Vector PreIntegrateIMUObservations_delta_vel(const Vector3& msr_acc_t,
const double msr_dt, const Vector3& delta_angles, const Vector3& delta_vel_in_t0){
// Note: all delta terms refer to an IMU\sensor system at t0
Vector body_t_a_body = msr_acc_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
}
Vector PreIntegrateIMUObservations_delta_angles(const Vector3& msr_gyro_t,
const double msr_dt, const Vector3& delta_angles){
// Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles) * Rot3::Expmap( msr_gyro_t * msr_dt );
Vector result = Rot3::Logmap(R_t_to_t0);
return result;
}
ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
@ -423,6 +440,75 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
/* ************************************************************************* */
TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
{
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
// 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
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
// 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 theta_i = preintegrated.thetaRij(); // before adding new measurement
const Vector3 deltaVij = preintegrated.deltaVij();// before adding new measurement
Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
body_P_sensor, Factual, Gactual);
// Compute expected F
Matrix H_vel_angles_expected =
numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel,
newMeasuredAcc, newDeltaT, _1, deltaVij), theta_i);
Matrix H_angles_angles_expected =
numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles,
newMeasuredOmega, newDeltaT, _1), theta_i);
Matrix Fexpected(9,9);
Fexpected << I_3x3, I_3x3 * newDeltaT, Z_3x3,
Z_3x3, I_3x3, H_vel_angles_expected,
Z_3x3, Z_3x3, H_angles_angles_expected;
// verify F
EXPECT(assert_equal(Fexpected, Factual));
// Compute expected G
Matrix H_vel_noiseAcc_expected =
numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel,
_1, newDeltaT, theta_i, deltaVij), newMeasuredAcc);
Matrix H_angles_noiseOmega_expected =
numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles,
_1, newDeltaT, theta_i), newMeasuredOmega);
Matrix Gexpected(9,9);
// [integrationError measuredAcc measuredOmega]
Gexpected << I_3x3 * newDeltaT, Z_3x3, Z_3x3,
Z_3x3, H_vel_noiseAcc_expected, Z_3x3,
Z_3x3, Z_3x3, H_angles_noiseOmega_expected;
// verify G
EXPECT(assert_equal(Gexpected, Gactual,1e-7));
}
//#include <gtsam/linear/GaussianFactorGraph.h>
///* ************************************************************************* */
//TEST( ImuFactor, LinearizeTiming)