diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index ade4b3dd7..47d8ab863 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -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; diff --git a/gtsam_unstable/slam/ImuFactor.h b/gtsam_unstable/slam/ImuFactor.h index 8ba169770..c3753e5e7 100644 --- a/gtsam_unstable/slam/ImuFactor.h +++ b/gtsam_unstable/slam/ImuFactor.h @@ -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 body_P_sensor = boost::none) : - Base(model, pose_i, vel_i, pose_j, vel_j, bias), + boost::optional 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), diff --git a/gtsam_unstable/slam/tests/testCombinedImuFactor.cpp b/gtsam_unstable/slam/tests/testCombinedImuFactor.cpp index 370ccc3b9..3f6d6c4db 100644 --- a/gtsam_unstable/slam/tests/testCombinedImuFactor.cpp +++ b/gtsam_unstable/slam/tests/testCombinedImuFactor.cpp @@ -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); diff --git a/gtsam_unstable/slam/tests/testImuFactor.cpp b/gtsam_unstable/slam/tests/testImuFactor.cpp index 9601e778e..82f9e3a88 100644 --- a/gtsam_unstable/slam/tests/testImuFactor.cpp +++ b/gtsam_unstable/slam/tests/testImuFactor.cpp @@ -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(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(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(