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

Taking ImuFactor noise model automatically from preintegrated measurement covariance

release/4.3a0
Richard Roberts 2013-08-09 18:50:14 +00:00
parent 0cb6e97803
commit 1eae17561d
4 changed files with 10 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam_unstable/gtsam_unstable.h
View File

@ -646,11 +646,10 @@ class ImuFactorPreintegratedMeasurements {
virtual class ImuFactor : gtsam::NonlinearFactor { 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, 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::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
const gtsam::noiseModel::Base* model);
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, 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::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 // Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;

4
gtsam_unstable/slam/ImuFactor.h
View File

@ -312,8 +312,8 @@ namespace gtsam {
/** Constructor */ /** Constructor */
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, 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 PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
const SharedNoiseModel& model, boost::optional<const Pose3&> body_P_sensor = boost::none) : boost::optional<const Pose3&> body_P_sensor = boost::none) :
Base(model, pose_i, vel_i, pose_j, vel_j, bias), Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias),
preintegratedMeasurements_(preintegratedMeasurements), preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity), gravity_(gravity),
omegaCoriolis_(omegaCoriolis), omegaCoriolis_(omegaCoriolis),

3
gtsam_unstable/slam/tests/testCombinedImuFactor.cpp
View File

@ -206,8 +206,7 @@ TEST( CombinedImuFactor, ErrorWithBiases )
// Create factor // 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);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov); 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); CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis, Combinedmodel);

13
gtsam_unstable/slam/tests/testImuFactor.cpp
View File

@ -182,8 +182,7 @@ TEST( ImuFactor, Error )
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // 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);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
@ -265,8 +264,7 @@ TEST( ImuFactor, ErrorWithBiases )
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // 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);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
SETDEBUG("ImuFactor evaluateError", false); SETDEBUG("ImuFactor evaluateError", false);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
@ -320,7 +318,7 @@ TEST( ImuFactor, PartialDerivativeExpmap )
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(boost::bind( 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); const Matrix3 Jr = ImuFactor::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
@ -343,7 +341,7 @@ TEST( ImuFactor, PartialDerivativeLogmap )
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(boost::bind( Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(boost::bind(
&evaluateLogRotation, thetahat, _1), deltatheta); &evaluateLogRotation, thetahat, _1), LieVector(deltatheta));
const Vector3 x = thetahat; // parametrization of so(3) const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ 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); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // 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);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
// Expected Jacobians // Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>( Matrix H1e = numericalDerivative11<Pose3>(