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

merge with upstream

release/4.3a0
Russell Buchanan 2021-01-19 10:54:52 +00:00
parent bb662f0cb4 fd43d817df
commit 07244bbdea
1 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
tests/testImuPreintegration.cpp
View File

@ -34,7 +34,7 @@ using namespace gtsam;
* acceleration input. * acceleration input.
*/ */
TEST(TestImuPreintegration, LoadedSimulationData) { TEST(TestImuPreintegration, LoadedSimulationData) {
Eigen::Vector3d finalPos; Vector3 finalPos(0, 0, 0);
vector<ImuMeasurement> imuMeasurements; vector<ImuMeasurement> imuMeasurements;
@ -70,10 +70,8 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
// Assume a Z-up navigation (assuming we are performing optimization in the // Assume a Z-up navigation (assuming we are performing optimization in the
// IMU frame). // IMU frame).
boost::shared_ptr<PreintegratedCombinedMeasurements::Params> auto imuPreintegratedParams =
imuPreintegratedParams = PreintegratedCombinedMeasurements::Params::MakeSharedU(gravity);
PreintegratedCombinedMeasurements::Params::MakeSharedU(
gravity);
imuPreintegratedParams->accelerometerCovariance = imuPreintegratedParams->accelerometerCovariance =
I_3x3 * pow(accNoiseSigma, 2); I_3x3 * pow(accNoiseSigma, 2);
imuPreintegratedParams->biasAccCovariance = I_3x3 * pow(accBiasRwSigma, 2); imuPreintegratedParams->biasAccCovariance = I_3x3 * pow(accBiasRwSigma, 2);
@ -84,11 +82,11 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
// Initial state // Initial state
Pose3 priorPose; Pose3 priorPose;
Vector3 priorVelocity; Vector3 priorVelocity(0, 0, 0);
imuBias::ConstantBias priorImuBias; imuBias::ConstantBias priorImuBias;
PreintegratedCombinedMeasurements imuPreintegrated; PreintegratedCombinedMeasurements imuPreintegrated;
Eigen::Vector3d position; Vector3 position(0, 0, 0);
Eigen::Vector3d velocity; Vector3 velocity(0, 0, 0);
NavState propState; NavState propState;
NavState initialNavState(priorPose, priorVelocity); NavState initialNavState(priorPose, priorVelocity);
@ -97,8 +95,8 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
priorImuBias = imuBias::ConstantBias(Eigen::Vector3d(0,0,0), priorImuBias = imuBias::ConstantBias(Eigen::Vector3d(0,0,0),
Eigen::Vector3d(0,0,0)); Eigen::Vector3d(0,0,0));
imuPreintegrated = PreintegratedCombinedMeasurements( imuPreintegrated =
imuPreintegratedParams, priorImuBias); PreintegratedCombinedMeasurements(imuPreintegratedParams, priorImuBias);
// start at 1 to skip header // start at 1 to skip header
for (size_t n = 1; n < imuMeasurements.size(); n++) { for (size_t n = 1; n < imuMeasurements.size(); n++) {