Taking ImuFactor noise model automatically from preintegrated measurement covariance
parent
0cb6e97803
commit
1eae17561d
|
@ -646,11 +646,10 @@ class ImuFactorPreintegratedMeasurements {
|
|||
|
||||
virtual class ImuFactor : gtsam::NonlinearFactor {
|
||||
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
|
||||
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
|
||||
const gtsam::noiseModel::Base* model);
|
||||
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
|
||||
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
|
||||
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
|
||||
const gtsam::noiseModel::Base* model, const gtsam::Pose3& body_P_sensor);
|
||||
const gtsam::Pose3& body_P_sensor);
|
||||
|
||||
// Standard Interface
|
||||
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
|
||||
|
|
|
@ -312,8 +312,8 @@ namespace gtsam {
|
|||
/** Constructor */
|
||||
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||
const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
const SharedNoiseModel& model, boost::optional<const Pose3&> body_P_sensor = boost::none) :
|
||||
Base(model, pose_i, vel_i, pose_j, vel_j, bias),
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none) :
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
|
|
|
@ -206,8 +206,7 @@ TEST( CombinedImuFactor, ErrorWithBiases )
|
|||
|
||||
|
||||
// Create factor
|
||||
noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov);
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
|
||||
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, Combinedmodel);
|
||||
|
|
|
@ -182,8 +182,7 @@ TEST( ImuFactor, Error )
|
|||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5));
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
|
||||
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
|
||||
|
||||
|
@ -265,8 +264,7 @@ TEST( ImuFactor, ErrorWithBiases )
|
|||
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5));
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
|
||||
SETDEBUG("ImuFactor evaluateError", false);
|
||||
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
|
||||
|
@ -320,7 +318,7 @@ TEST( ImuFactor, PartialDerivativeExpmap )
|
|||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(boost::bind(
|
||||
&evaluateRotation, measuredOmega, _1, deltaT), biasOmega);
|
||||
&evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega));
|
||||
|
||||
const Matrix3 Jr = ImuFactor::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
|
||||
|
||||
|
@ -343,7 +341,7 @@ TEST( ImuFactor, PartialDerivativeLogmap )
|
|||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(boost::bind(
|
||||
&evaluateLogRotation, thetahat, _1), deltatheta);
|
||||
&evaluateLogRotation, thetahat, _1), LieVector(deltatheta));
|
||||
|
||||
const Vector3 x = thetahat; // parametrization of so(3)
|
||||
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
|
||||
|
@ -529,8 +527,7 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
|
|||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5));
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
|
||||
// Expected Jacobians
|
||||
Matrix H1e = numericalDerivative11<Pose3>(
|
||||
|
|
Loading…
Reference in New Issue